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Abstract 

In  the  recent  past,  a  shift  has  taken  place  from  manned  to  unmanned  Intelli¬ 
gence,  Surveillance,  and  Reconnaissance  (ISR)  missions.  This  shift  has  lead  to  an 
increase  in  the  number  of  unmanned  vehicles  (UV)  operating  in  a  theater.  Addition¬ 
ally,  removal  of  the  crew  allows  for  a  reduction  in  vehicle  scale,  which  leads  to  an 
increased  ability  to  operate  in  GPS  degraded  environments.  With  the  loss  of  GPS 
signals  the  vehicles  must  rely  on  Inertial  Navigation  Systems  (INS)  which  when  re¬ 
duced  to  an  appropriate  size  are  inherently  inaccurate.  This  research  endeavors  to 
exploit  three  attributes  of  increased  UV  use  for  ISR  missions.  These  attributes  are: 
increased  numbers  of  UVs,  on-board  vision,  and  wireless  communications. 

This  research’s  focus  is  the  development  and  validation  of  a  cooperative  navi¬ 
gation  system  based  on  the  measurement  of  UV  position  relative  to  shared  landmark 
position  estimates.  Each  UV  in  the  network  locates  landmarks  using  it’s  on-board 
vision  system  and  transmits  the  data  to  all  other  system  UVs.  After  receiving  data 
from  the  other  UVs,  the  system  fuses  the  landmarks  with  on-board  measurements 
using  a  federated  filter  architecture. 

The  system  is  evaluated  using  Matlab®  simulation.  Simulations  of  the  coopera¬ 
tive  system,  with  and  without  ranging,  are  compared  to  a  non-cooperative  simulation. 
The  comparison  is  performed  using  four  platform  motion  scenarios:  stationary,  linear, 
angular,  and  full  motion.  The  simulation  results  demonstrate  position  error  estimate 
improvements  of  0.5  cm  to  1cm.  Additionally,  the  stationary  and  linear  motion  sce¬ 
narios  demonstrate  attitude  observability  difficulties  eliminated  by  the  introduction 
of  angular  motion. 
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I.  Introduction 


his  thesis  outlines  a  research  effort  focused  on  expanding  previous  research  into 


_L  the  fusion  of  optical  and  inertial  sensors  for  robust,  autonomous  navigation 
to  multiple  platforms.  This  research  effort  is  motivated  by  the  recent  increase  in  the 
number  of  small  unmanned  vehicles  with  the  ability  to  operate  in  environments  where 
external  navigation  reference  sources  are  unavailable. 

The  development  of  Unmanned  Aerial  Vehicles  (UAVs)  began  soon  after  the  first 
manned  flight.  Originally,  this  progression  was  constrained  to  flying  bomb  or  cruise 
missile  design  and  testing.  However,  UAV  development  quickly  expanded  into  Intelli¬ 
gence,  Surveillance,  and  Reconnaissance  (ISR)  missions  following  the  1959  shoot-down 
of  Francis  Gary  Powers  over  Russia  [4],  In  addition  to  advancements  in  UAV  tech¬ 
nology,  numerous  Unmanned  Ground  (UGVs)  and  Maritime  (UMVs)  vehicles  have 
been  developed  for  ISR  work.  The  evolution  of  unmanned  systems  has  lead  to  a  shift 
from  manned  to  unmanned  ISR  missions  which  has  in-turn  lead  to  an  increase  in  the 
number  of  Unmanned  Vehicles  (UVs)  operating  in  a  theater. 

“Today,  we  now  have  more  than  5,000  UAVs,  a  25-fold  increase  since  2001.  But 
in  my  view,  we  can  do  -  and  we  should  do  -  more  to  meet  the  needs  of  men  and  women 
fighting  in  the  current  conflicts...”  [6]  With  this  statement,  during  a  speech  to  the  Air 
War  College  in  April  2008,  Defense  Secretary  Gates  called  for  further  increase  in  the 
number  of  UAVs  fielded  by  the  Air  Force. 

The  development  of  UV  technologies  has  removed  the  pilot  and  crew  from  the 
vehicle  to  safe  locations.  As  a  result  the  scale  of  these  vehicles  may  be  reduced  con¬ 
siderably.  This  reduction  in  scale  allows  for  the  increased  operation  of  UVs  in  Global 


1 


Positioning  System  (GPS)  degraded  environments.  These  environments  include  but 
are  not  limited  to,  urban  canyons,  caves,  underwater,  and  building  interiors.  With 
the  loss  of  GPS  signal  the  vehicles  are  forced  to  rely  on  Inertial  Navigation  Systems 
(INS)  which,  when  reduced  to  an  appropriate  size,  are  inherently  inaccurate. 

As  the  reliance  on  these  vehicles  increases  so  does  the  desire  for  ever  more  accu¬ 
rate  navigation  and  targeting.  “The  ability  to  positively  identify  and  precisely  locate 
military  targets  in  real-time  is  a  current  shortfall  with  DoD  UAS.”  In  response,  the 
Department  of  Defense  (DoD)  has  designated  reconnaissance  and  precision  target¬ 
ing  as  two  of  their  top  three  priorities  for  UVs  in  all  three  categories  (UAV,  UGV, 
UMV)  [14].  The  ability  to  precisely  locate  targets  is  dependent  on  the  accuracy  of 
the  targeting  platform’s  navigation  state.  Therefore,  any  increase  in  the  accuracy  of 
the  navigation  state  also  improves  the  location  estimate  of  a  target. 

The  concept  of  this  thesis  is  inspired  by  three  factors.  First,  previous  research 
to  exploit  the  availability  of  visual  data  on  board  a  UV  demonstrates  the  ability  of 
vision  aiding  to  improve  the  navigation  solution.  Next,  the  increased  numbers  of  UVs 
operating  within  a  theater  provides  for  multiple  sources  of  visual  data.  Finally,  the 
capability  of  wireless  communication,  inherent  in  the  vast  majority  of  UVs  designed 
today,  provides  a  readily  available  means  of  sharing  visual  data  between  platforms. 

1.1  Problem  Definition 

The  measurement  of  a  target  location  is  a  function  of  the  position  of  the  mea¬ 
surement  device,  the  alignment  of  the  measurement  device  to  the  platform,  and  the 
accuracy  of  the  measurement  device.  Therefore,  the  accuracy  of  the  navigation  so¬ 
lution  impacts  the  ability  of  the  platform  to  precisely  locate  targets.  This  research 
focuses  on  the  accuracy  of  the  navigation  solution  and  leaves  the  alignment  and  ac¬ 
curacy  of  the  measurement  device  to  other  research  efforts. 
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For  the  purposes  of  this  thesis,  navigation  is  defined  as  the  determination  of 
position,  velocity,  and  attitude  relative  to  a  predefined  reference  frame.  The  most 
common  method  for  determining  a  navigation  solution,  in  the  absence  of  GPS,  is  the 
INS. 


1.1.1  Inertial  Navigation  Systems.  An  INS  is  composed  of  two  types  of 
passive  sensors:  accelerometers  and  gyroscopes.  Accelerometers  measure  the  specific 
force  acting  on  the  platform  in  the  sensitive  axis  direction  of  the  accelerometer,  while 
gyroscopes  measure  the  angular  rotation  rate  of  the  platform  with  respect  to  an 
inertial  reference  frame.  The  measurements  of  both  devices  are  corrupted  by  errors 
which  are  discussed  further  in  Chapter  II,  Sections  2.4.1  and  2.4.2  respectively.  The 
combination  of  measurements  from  these  two  devices  provide  a  navigation  solution 
relative  to  an  initial  position. 

The  solution  of  an  INS  will  drift  away  from  the  true  solution  as  a  function 
of  time,  due  to  the  errors  inherent  in  the  sensors.  This  drift  may  be  reduced  by  the 
incorporation  of  additional  measurements  to  the  system.  Previous  research  has  shown 
visual  aiding  of  an  INS  reduces  this  drift. 

1.1.2  Vision  Aiding.  Vision  aiding  is  the  use  of  image  sensors  to  measure 
the  position  of  features  within  a  sequence  of  images  to  measure  the  linear  and  angular 
drift  of  the  platform.  Previous  research  into  vision  aiding  of  an  INS  is  discussed  further 
in  Chapter  II,  Section  2.6. 

1.2  Research  Contribution 

The  primary  contribution  of  this  research  is  the  application  of  distributed  fil¬ 
tering  techniques  to  existing  research  into  the  coupling  of  image  and  inertial  sensors. 
This  expands  the  current  capabilities  of  vision-aided  navigation  to  multiple  platform 
cooperative  systems,  improving  the  accuracy  of  individual  platform  navigation  solu¬ 
tions. 
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1.3  Assumptions 

This  research  is  an  extrapolation  of  the  image  and  inertial  fusion  navigation  al¬ 
gorithm  developed  in  [20].  Therefore,  the  following  four  assumptions  of  the  algorithm 
are  required  for  this  research: 

•  the  system  includes  a  strapdown  inertial  sensor  which  is  rigidly  mounted  relative 
to  one  or  more  cameras, 

•  the  cameras  capture  images  at  known  times  relative  to  inertial  measurements, 

•  the  initial  navigation  state  and  accuracy  statistics  are  known,  and 

•  the  estimated  distance  to  objects  in  the  scene  are  available. 

The  estimated  distance  to  objects  may  be  provided  from  exploitation  of  knowledge  of 
the  world  or  through  binocular  stereopsis. 

In  addition  to  these  assumptions,  the  multiple  platform  nature  of  this  research 
requires: 

•  image  feature  descriptors  generated  on  each  platform  are  of  the  same  format, 

•  feature  positions  are  computed  in  the  11-frame, 

•  a  common  communication  protocol  is  available  to  all  platforms  for  the  transfer 
of  data,  and 

•  to  simplify  implementation  of  the  cooperative  navigation  system  (CNS),  range 
measurements  between  platforms  are  acquired  at  the  same  moment  as  a  platform 
position  is  generated. 

These  additional  assumptions  are  discussed  in  detail  in  Chapter  III. 
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1.4  Overview  of  Thesis 

This  thesis  is  organized  as  follows.  Chapter  II  provides  the  mathematical  back¬ 
ground  for  the  cooperative,  multiple  platform  vision  aided  navigation  system.  This 
includes  mathematical  notation,  coordinate  frames  and  transformations,  inertial  nav¬ 
igation,  extended  Kalman  and  federated  filtering,  and  previous  vision  aiding  and 
multiple  vehicle  research.  Chapter  III  describes  the  methodology  used  to  develop 
the  cooperative  vision  aided  navigation  system.  Chapter  IV  presents  the  simulation 
data  and  analyzes  the  results.  Finally,  Chapter  V  presents  conclusions  regarding  the 
theory  and  recommendations  for  future  research. 
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II.  Background 


This  chapter  reviews  the  concepts  and  background  required  to  fully  develop  the 
problem  of  multiple  unmanned  vehicle,  cooperative,  vision-aided  navigation. 
The  chapter  opens  with  a  definition  of  the  mathematical  notation  used  throughout 
the  document.  Next,  the  reference  frames  used  for  inertial  navigation  are  defined.  A 
short  discussion  of  coordinate  transformations  presents  the  mathematical  techniques 
required  to  properly  format  coordinates  for  any  reference  frame.  Then,  the  concepts 
of  INS  operation  are  briefly  discussed  with  an  emphasis  on  Micro- miniature  Elec¬ 
tromechanical  Systems  (MEMS)  units.  A  brief  discussion  of  Kalman  filtering  follows, 
providing  a  foundation  for  vision-aiding,  federated  filtering,  and  this  research.  Fol¬ 
lowing  this,  the  background  of  INS  vision-aiding  and  the  work  performed  in  this  field 
are  explored  to  develop  the  vision  framework  of  the  problem.  Next,  the  federated 
filter  is  defined  to  provide  the  architecture  upon  which  the  sharing  of  information, 
and  navigation  state  estimates  are  developed.  Finally,  the  concept  of  multiple  vehicle 
cooperation  is  presented  to  provide  a  foundation  for  the  development  of  a  network  of 
unmanned  agents. 

2.1  Mathematical  Notation 

The  mathematical  notation  used  throughout  this  research  is  described  in  Ta¬ 
ble  2.1. 

2. 2  Reference  Frames 

Precise  definition  of  coordinate  reference  frames  is  a  vital  prerequisite  for  inertial 
navigation.  For  the  purposes  of  this  thesis,  five  reference  frames  are  defined  based 
on  [7]  and  [19].  All  of  the  reference  frames  defined  are  right-handed  orthonormal 
coordinated  systems  in  3ft3. 

The  true  inertial  reference  frame  ( 1-frame )  has  no  fixed  origin  or  orientation;  is 
non-rotating  and  non- accelerating.  This  is  a  theoretical  reference  frame  in  which  the 
Newtonian  laws  of  physics  apply. 
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Table  2.1:  Mathematical  Notation. 


Type 

Description 

Example 

Scalars 

Scalar  variables  are  designated  with  italics. 

x  or  X 

Vectors 

Vectors  are  denoted  by  lower  case  bold. 

X 

Matrices 

Matrices  are  denoted  by  uppercase  bold. 

X 

Transpose 

The  transpose  of  a  matrix  or  vector  is  des¬ 
ignated  with  a  superscript  capital  T. 

xT  or  Xr 

Estimates 

Estimates  of  random  variables  are  identi¬ 
fied  with  the  hat  character. 

X 

Calculated  Variables 

Variables  corrupted  by  errors  are  denoted 
with  the  tilde  character. 

X 

Nominal  Values 

Nominal  values  are  denoted  with  a  bar. 

X 

Direction  Cosine  Matrix 
(DCM) 

DCMs  are  designated  by  a  bold  capital  C 
with  a  subscript  designating  the  originat¬ 
ing  coordinate  frame  and  a  superscript  des¬ 
ignating  the  resulting  coordinate  frame. 

Frame  of  Reference 

Vectors  expressed  in  a  specific  reference 
frame  are  annotated  with  a  superscript  let¬ 
ter  representing  the  frame. 

pn 

Skew  Symmetric  Form 

Vectors  represented  in  skew  symmetric 
form  are  enclosed  in  parentheses  with  a 
cross  product  symbol. 

(xx) 
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Figure  2.1:  Body  Frame  of  Reference  [19] 

The  inertial  frame  ( i-frame )  is  a  non-rotating,  Earth-centered  frame  in  which 
the  z-axis  is  coincident  with  the  polar  axis.  Due  to  the  motion  of  the  Earth  around 
the  Sun,  and  the  motion  of  the  Sun  through  space,  the  i-frame  is  an  accelerating 
frame.  Additionally,  the  i-frame  is  non-rotational  with  respect  to  the  fixed  stars.  As 
the  I-frame  has  no  fixed  origin  or  orientation,  the  i-frame  is  designed  to  provide  these 
critical  components  of  a  reference  frame.  This  allows  measured  physical  values  to  be 
transformed  into  other  frames  of  reference  as  discussed  in  Section  2.3. 

The  Earth  frame  ( e-frame )  originates  at  the  center  of  the  Earth  and  is  fixed 
with  respect  to  the  surface.  The  z-axis  is  aligned  with  the  geographic  polar  axis.  The 
x-axis  lies  along  the  intersection  of  the  Greenwich  meridian  and  equatorial  planes. 
The  e-frame  therefore  rotates  in  relation  to  the  i-frame. 

The  navigation  frame  ( n-frame )  is  centered  about  a  fixed  point  on  the  navigating 
body.  The  x-axis  lies  within  the  plane  created  by  the  Earth’s  polar  axis  and  the  origin 
of  the  n-frame,  pointing  towards  the  north  pole.  The  z-axis  points  in  the  direction  of 
gravitational  acceleration. 

The  body  frame  (b- frame)  is  centered  at  the  origin  of  the  n-frame.  However, 
the  axes  differ  from  the  n-frame.  The  axes  are  aligned  with  the  roll,  pitch  and  yaw 
axes  of  an  aircraft  as  depicted  in  Figure  2.1. 

In  addition  to  these  five  reference  frames,  a  system  designer  may  develop  any 
new  arbitrary  reference  frame  which  will  simplify  the  data  processing  for  the  system. 
For  the  purpose  of  this  thesis,  the  set  of  these  possible  frames  will  be  annotated  as 
the  li-frame  local  frames,  or  as  Ci-frame  for  camera  frames,  where  i  —  1 . . .  n  and  n  is 
the  total  number  of  local  or  camera  frames. 
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The  next  section  discusses  the  means  to  transform  coordinates  between  these 


coordinate  systems. 


2. 3  Coordinate  Transformations 

As  discussed  in  [19]  coordinate  transformations  describe  the  relationship  be¬ 
tween  two  reference  frames  and  are  classified  as  either  three  or  four-parameter  trans¬ 
formations.  Three-parameter  coordinate  transformations  contain  a  singularity  at  a 
pitch  angle  of  90°.  Therefore,  this  research  will  use  the  four-parameter  DCM  coordi¬ 
nate  transformation. 


The  DCM  is  a  3x3  matrix  representing  the  unit  vector  of  the  originating  frame 
projected  along  the  axis  of  the  resulting  frame.  The  DCM  is  written  in  component 
form  as: 


c;  = 


Cll 

c  12 

Cl3 

C21 

C22 

C23 

C31 

C32 

C33 

(2.1) 


The  elements  c%3  represents  the  cosine  of  the  angle  between  the  i-axis  of  originating 
reference  frame  and  the  j-axis  of  the  resulting  frame. 


The  DCM  is  propagated  in  time  through: 


c:  =  cr0n°0 


(2.2) 


where 


fj°  = 

uro 


0 

UJZ 
— UJ ■ 


-O 


y 


LOq, 


0  —ux 
UJr  0 


(2.3) 


n  T 


is  the  skew  symmetric  form  of  the  angular  rate  vector  u)°Q  =  u>x  uy  u>z  ,  which 
represents  the  angular  turn  rate  of  the  originating  frame  with  respect  to  the  resulting 
frame  expressed  in  the  axes  of  the  originating  frame. 
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In  the  absence  of  a  GPS  signal,  an  INS  may  provide  the  coordinates  for  trans¬ 
formation. 

2-4  Inertial  Navigation 

In  this  section,  the  basic  concepts  of  inertial  navigation  are  discussed.  This 
discussion  includes  strapdown  INS  sensors,  related  errors,  available  measurements, 
and  differential  error  equations. 

2-4-1  Accelerometers.  Acceleration  due  to  inertial  motion  and  acceleration 
due  to  gravity  are  indistinguishable  as  described  by  Einstein’s  Theory  of  Special 
Relativity  [5].  Therefore,  accelerometers  measure  the  specific  force  applied  to  them 
in  the  direction  of  alignment.  This  measurement  is  the  difference  between  the  inertial 
motion  and  gravitational  accelerations,  defined  as 

f  =  p  -  g  (2.4) 

where  f  is  the  specific  force,  p  is  the  inertial  position  and  g  is  the  acceleration  due  to 
gravity. 

The  measurement  may  be  contaminated  by  errors  induced  by  the  design  of  the 
accelerometer  [19].  The  specific  force  measurement  (fb)  may  be  expressed  as: 

fx  —  (1  +  Sx)fx  +  Myfy  +  Mzfbz  +  Bvfbfb  +  a6  +  rjb  (2.5) 

where  fb,  fb,  fb  are  the  true  specific  forces  applied  in  the  sensitive  and  cross  axes  and 
the  remaining  errors  are: 

•  Measurement  Bias(ab):  An  additive  error  arising  as  a  direct  result  of  the 
characteristics  of  the  accelerometer’s  component  parts.  This  error  may  fluctuate 
slowly  with  time  and  may  be  compensated  for  using  mechanical  calibration 
techniques. 
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•  Scale  Factor (Sx):  A  multiplicative  error  caused  by  temperature  and  non-ideal 
behavior  of  system  components.  As  with  measurement  bias,  scaling  factors  may 
by  compensated  for  using  calibration. 

•  Cross  Axis  Coupling(M?/  and  Mz):  An  additive  error  caused  by  the  inclusion 
of  forces  applied  to  the  device  in  the  non-sensitive  axes.  This  error  may  have 
root  cause  in  the  design  of  the  device  or  in  the  misalignment  of  the  device  with 
the  vehicle. 

•  Vibro-pendulous  Error (Bv):  An  error  due  to  vibration  in  the  sensitive  axis. 
This  error  may  be  reduced  by  isolation  of  the  device  from  vibration  sources. 

•  Random  Bias(r/6):  An  error  caused  by  instabilities  within  the  device. 

In  addition  to  these  errors,  the  final  measurement  may  be  corrupted  by  errors  in  the 
gyroscopes  and  inaccuracies  of  the  gravity  model  used  to  remove  the  gravitational 
acceleration  from  the  specific  force  equation. 

2.4.2  Gyroscopes.  A  spinning  wheel  or  rotor,  by  virtue  of  its  angular  mo¬ 
mentum  vector,  tends  to  maintain  the  direction  of  its  spin  axis  relative  to  inertial 
space  [19].  This  defines  a  reference  direction  which  remains  fixed  in  the  i- frame. 

The  gyroscopes  used  in  strapdown  systems  measure  the  angular  rate  of  a  plat¬ 
form,  relative  to  inertial  space,  about  the  gyroscope  input  axis.  The  measurement 
may  be  contaminated  by  errors  induced  by  the  design  of  the  gyroscope  [19].  The 
angular  rate  measurement  (cb^)  may  be  expressed  as: 

<  =  (1  +  +  Myu)biby  +  Mzu 4  +  Bgxfbx  +  Bgzfl  +  BaxzfbJbz  +  b b  +  rjb  (2.6) 

where  ujbhi ,  ubb  ,  and  cobbz  are  the  true  turn  rates  about  the  input,  output,  and  spin 
axes  respectively;  fb,  and  fb  are  the  true  specific  forces  applied  along  the  input  and 
spin  axes  and  the  remaining  errors  are: 
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•  Fixed  Bias  (b6):  A  sensor  error  present  in  the  absence  of  an  applied  input 
rotation.  A  consequence  of  multiple  effects,  including  residual  torques  from 
flexible  leads,  spurious  magnetic  fields,  and  temperature  gradients. 

•  Acceleration  Dependent  Bias  ( Bfy  and  Bfz ):  An  error  proportional  to  the 
magnitude  of  the  applied  acceleration.  Apparent  in  spinning  mass  gyroscopes 
as  a  result  of  an  unbalanced  rotor  mass. 

•  Anisoelastic  Bias  ( Baxz ):  An  error  proportional  to  the  product  of  acceleration 
along  orthogonal  pairs  of  axes.  Apparent  in  spinning  mass  gyroscopes  and 
caused  by  the  rotor  suspension  structure. 

•  Scale  Factors  ( Sx ):  An  error  caused  by  imperfections  and  temperature  fluctu¬ 
ations  in  the  pickoff  and  nulling  components  of  the  gyroscope. 

•  Cross  Coupling  ( My  and  Mz):  Errors  due  to  the  non-orthogonality  of  the 
sensor  axes. 

•  Zero-Mean  Bias  (77* ) :  An  error  caused  by  instabilities  in  the  gyroscope  with 
short  correlation  times. 

The  accelerometer  and  gyroscope  measurements  equations  are  now  used  to  com¬ 
pute  the  differential  error  equations. 

2-4-3  Differential  Error  Equations.  The  differential  error  equations  are  com¬ 
puted  in  three  parts;  inertial  sensor  errors,  attitude  error,  and  position  and  velocity. 
The  results  are  then  combined  to  generate  the  state-space  error  model. 

2-4-3. 1  Inertial  Sensor  Errors.  The  inertial  sensor  errors  are  modeled 
as  a  bias  with  added  random  noise.  The  accelerometer  and  gyroscope  measurement 
models  are: 


f 6  +  afc  +  wba 

(2.7) 

4  +  bb  +  wbb 

(2.8) 
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where  and  w \  are  additive  white  Gaussian  noise  processes,  ab  is  the  accelerometer 
bias,  and  is  the  gyroscope  bias. 

The  accelerometer  and  gyroscope  biases  are  modeled  as  first-order  Gauss-Markov 
processes  expressed  by: 


ab  = - a6  +  w* 

q-  U'bia 

1  a 


bb  = - bb  +  wb 


(2.9) 

(2.10) 


where  ra  and  t&  are  the  accelerometer  and  gyroscope  time  constants,  and  the  processes 
are  driven  by  the  white  noise  terms  wb  and  w? 

u-bias  ubias 


2. 4- 3. 2  Attitude  Error.  The  development  of  the  attitude  differential 
error  equations  begins  with  the  attitude  error  vector,  which  is  modeled  as: 


'Ipn 
Ipe 
Ip d 


(2.11) 


where  pin,  ipe,  and  ipd  are  small  angles  relative  to  the  north,  east  and  down  axes  of 
the  n-frame  respectively. 

Due  to  the  small  angle  assumption,  the  b-frame  to  n-frame  DCM  can  be  for¬ 
mulated  as: 

C?«[t-(^x)]C?  (2.12) 

Taking  the  derivative  of  (2.12)  with  respect  to  time  and  solving  for  (ipx)  results  in: 


(Vix)  =  [I  -  (t/>x)] -  CnACbn  (2.13) 
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Next,  the  b-frame  to  n-frame  rotation  rate  vector  is  defined  as: 


u =  —  C  Cn<jje 

nb  ibm  n  e  te 


(2.14) 


where  oj\bm  is  the  rate  vector  measured  by  the  gyroscopes. 

Substituting  (2.8),  (2.12),  and  (2.14)  into  (2.13)  and  eliminating  second-order 
terms  results  in  the  angular  differential  error  equation: 


-[(CXe)x]</> 


civ  -  cnM 


(2.15) 


2. 4- 3. 3  Position  and  Velocity  Error.  The  position  and  velocity  differ¬ 
ential  error  equation  development  begins  with  computing  the  position  error. 

The  n-frame  velocity  is  defined  as: 

pn  =  vn  (2.16) 

and  the  position  differential  error  equation  is: 

Spn  =  5vn  (2.17) 


Next,  the  calculation  of  the  velocity  error  begins  with  the  definition  of  the 
position  in  the  i- frame: 

p*  =  Cl  [p*  +  C*p"]  (2.18) 

where  p[)  is  the  location  of  the  origin  of  the  local  navigation  frame,  relative  to  the 
e-frame. 

The  acceleration,  in  i-frame  coordinates,  is  calculated  by  taking  the  second 
derivative  using  the  chain  rule: 


P 


=  cicenpn  +  2c*n?ec®pn  +  c 


>o  +  C'p"] 


(2.19) 
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Appling  (2.4)  to  (2.19)  and  solving  for  the  n- frame  acceleration  provides: 


p“  =  f"  -  2Cjn'ec'pn  -  c“(ny2  [ps  +  c;P”]  +  g- 


(2.20) 


Then,  substituting  (2.16)  into  (2.20)  and  transforming  the  specific  force  into  the 
b-frame  yields: 


v”  =  c;r‘  -  2Cj!2“ec'v"  -  c”(ny2  [pg  +  c'P"]  +  g“ 


(2.21) 


The  centripetal  acceleration  and  gravity  terms  are  combined  using  the  gradient 
of  the  gravity  potential: 


W(pe) 


GM 

l|pel! 


+ 


T  T 


+  H.O.T. 


(2.22) 


where  GM  is  the  gravitational  constant  of  Earth.  Substituting  (2.22)  into  (2.21) 
provides: 


=  c^r  -  2C[ 


nic 


evn  +  C 


>o  +  Cenpn] 


(2.23) 


As  the  velocity  calculation  is  corrupted  by  accelerometer  and  attitude  errors; 
substituting  the  position,  velocity,  attitude  and  accelerometer  measurement  errors 
equations  into  (2.23)  yields: 


vre  =  [I  -  (V>x)]C£(f6  +  ab  +  wba 


2C:tlfC  t(wn 


5vr 


Cnege  [pe0  +  Cenpn  +  Cen5p' 


(2.24) 


The  n-frame  acceleration  error  is  defined  as: 

5irn  =  -  vn 


(2.25) 
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Finally,  the  velocity  differential  error  equation  is  computed  by  substituting 
(2.21)  and  (2.24)  into  (2.20)  and  eliminating  second-order  terms: 


=  CneGCeJpn  -  2CneSleieCen5vn  +  (P x)V>  +  C^ab  +  C>b 


(2.26) 


where  G  is  the  gradient  of  the  gravity  vector,  which  is  calculated  as: 


g  =  P 1  [3p»(p')t  - 1]  -  (n. 


IP 


e  1 13 


6)2 

ieJ 


(2.27) 


2. 4-3. 4  State-space  Error  Model.  With  the  inertial  sensor,  attitude, 
position,  and  velocity  differential  error  equations  computed,  the  error  dynamics  are 
formulated  using  the  linear,  stochastic,  state-space  model  which  is  driven  by  white 
noise: 

5x{t)  =  F  (t)x(t)  +  G(t)w(f)  (2.28) 


The  navigation  error  state  vector  is  composed  of  the  position,  velocity,  attitude, 
accelerometer  bias,  and  gyroscope  bias  errors,  resulting  in  the  fifteen  element  vector: 


fix  = 


5pn 

3vn 

Sab 

Sbb 


(2.29) 


15x1 


The  noise  vector  is  composed  of  the  accelerometer  measurement,  gyroscope  mea¬ 
surement,  accelerometer  bias,  and  gyroscope  bias  noise  terms,  resulting  in  the  twelve 
element  vector: 
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w  = 


(2.30) 


Finally,  the  overall  differential  error  equation  is: 
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(2.31) 


These  equations  show  accelerometer  and  gyroscope  outputs  may  be  integrated 
to  estimate  the  change  in  position,  velocity,  attitude,  and  inertial  sensor  biases.  How¬ 
ever,  these  estimates  drift  rapidly,  on  the  order  of  90  meters  in  1  minute,  for  a  stan¬ 
dard  MEMS  IMU  position  error  [10].  The  drift  may  be  constrained  by  incorporating 
measurements  from  additional  sensors  using  a  Kalman  filter. 


2.5  Kalman  Filtering 

This  section  discusses  the  concepts  of  Kalman  filtering  by  developing  the  linear 
filter  equations  and  expanding  the  linear  equations  to  nonlinear  functions. 

2.5.1  Linear  Kalman  Filter.  The  linear  Kalman  filter  is  an  optimal,  recur¬ 
sive  data  processing  algorithm  [12]  designed  to  estimate  desired  quantities  from  data 
provided  by  a  noisy  environment. 

The  general  form  of  a  time-varying  linear  differential  equation  is: 

x(£)  =  F  (t)x(t)  +  B(£)u(i)  +  G(t)w(t)  (2.32) 
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where  x(t)  is  the  state  estimate,  F(t)  is  the  homogeneous  system  dynamics  matrix, 
B (t)  is  the  input  matrix,  u (t)  is  the  system  input  vector,  G  (t)  is  the  noise  transfor¬ 
mation  matrix,  and  w(t)  is  a  white  Gaussian  noise  process  of  zero-mean  and  strength 

Q  (t). 

The  white  noise  covariance  is  computed  as: 

E  {w(t)wT(t  +  r)}  =  Q (t)5(r)  (2.33) 

where  5(r)  is  the  Dirac  delta  function. 

The  maximum  a  posteriori  state  estimate  is  the  mean  value  and  is  denoted  by 
the  hat  character: 


x(t)  =  F{x(t)}  (2.34) 

=  &(t,t0)E{k(t0)}+  f  $(t,r)B(r)u(r)dr  (2.35) 

J  to 

where  <&(t,  to)  is  the  state  transition  matrix  which  satisfies  <F(t,to)  =  F(t)<fr(t,  to)  and 

*(to,  to)  =  I- 

The  mean  squared  value  of  x(t)  is: 


E  (x(t)xT(t)}  =  $(t,t0)E  {x(t0)xT  (t0)}  3>r(t,t0) 

+  /  $(t,r)G(r)Q(r)GT(r)$T(t,r)dr  (2.36) 


and  the  covariance  of  x(t)  is  derived  from  the  mean  squared  value  by  substituting 


E  (x(t)xr(t)}  =  P xx(t)  +  mx(t)m^(t) 

E  (x(t0)xT(t0)}  =  Pxx{t0)  +  mx(to)m^(t0) 
m  x(t)  =  $(t,t0)mx(t0) 


18 


(2.37) 

(2.38) 

(2.39) 


into  (2.36)  and  solving  for  P xx(t): 


P  xx(t)  =  ®(t,t0)Pxx(t0)$T  (Mo) 

+  f  &(t,  r)G(r)Q(r)Gri  (r)$i  (t,  r)dr 


(2.40) 


Using  (2.35)  and  (2.40)  the  state  estimate  and  covariance  are  propagated  from 
time  ti  to  ti+i\ 


x(ti+i)  =  &(ti+1,ti)k(tf)  +  /  $(U+i,r)B(r)u(r)dr  (2.41) 

Ju 

P(v+i)  =  ^(U+1,U)P(^)^T(U+1,U) 

+  [  1+1  $(ti+i,  t)G(t)Q(t)Gt(t)$t(Mi,  r)dr  (2.42) 


where  the  superscript  —  and  +  represent  the  instant  before  and  after  a  measurement 
update  respectively. 


A  set  of  measurements  z  is  a  linear  combination  of  the  m  measurements  of 
interest,  corrupted  by  an  uncertain  disturbance  v: 


z  (ti)  =  H  (U)x(U)  +  v(U)  (2.43) 

where  H(U)  is  the  observation  matrix  and  v(U)  is  a  zero  mean  white  Gaussian  noise 
vector  with  a  covariance  defined  as: 


E  (v(U)vT(U)}  =  R (ti)5ij 
where  StJ  is  the  Kroeneker  delta  function. 


(2.44) 
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The  state  estimates  and  covariance  immediately  after  a  measurement  update 


are  defined  as 


*(ft)  =  *(<i  )  +  K(ij)  [z (U)  -  H(ii)x(fj  )] 
P(t+)  =  P(V)  -  K(i,)H(f,)P(fr) 


where  K(£j)  is  the  Kalman  gain  matrix,  dehned  as 


K(fj)  =  P(V)Ht(£,)  H(£i)P(£t")HT(£i)  +  R(£* 


i  -i 


(2.45) 

(2.46) 


(2.47) 


2.5.2  Extended  Kalman  Filter.  The  linear  Kalman  filter  provides  sufficient 
estimates  for  many  real-world  systems;  however,  the  linear  filter  is  unable  to  provide 
an  adequate  answer  for  all  situations.  The  extended  Kalman  filter  (EKF)  [13]  is 
developed  to  estimate  these  nonlinear  cases. 

The  development  of  the  EKF  begins  with  the  definition  of  the  error  model.  The 
state  estimate  error  model  is  the  sum  of  the  nominal  trajectory  and  the  error  state: 

x(t)  =  x(t)  +  5x(t)  (2.48) 


The  nominal  trajectory  is  defined  as: 

x(t)  =  f[x(t),u  (£),£]  (2.49) 

Applying  (2.49)  and  the  linear  differential  equation: 

5x(t)  =  F  (t)5x(t)  +  Gw(f)  (2.50) 

to  (2.48)  results  in  the  nonlinear  dynamics  model: 

x(£)  +  5x(t)  =  f  [x(£),  u(£),  t]  +  F(t)5x(t)  +  Gw(f)  (2.51) 
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Then,  the  state  estimate  is  defined  as  the  sum  of  the  nominal  trajectory  and 
the  estimated  error: 

x(f)  =  x(t)  +  Sx(t)  (2.52) 

The  state  estimate  is  propagated  from  time  tf  to  t~+1  by  calculating  the  nominal 
trajectory  with  a  nonlinear  differential  equation  solver  using  the  state  estimate  at  tf 
as  the  initial  condition.  The  covariance  is  propagated  as  in  the  linear  Kalman  filter. 

A  measurement  update  is  performed  by  linearizing  the  nonlinear  update  equa¬ 
tion: 

z  (tf)  =  h[x(fi),  ti]  +  v(tf)  (2.53) 

about  the  nominal  trajectory  at  time  ti+1: 

z  (U)  =  h[x(fr+1),  ti+i]  +  H(ti+i)5x(ti+i)  +  v(ti+i)  (2.54) 


where  H(£)  is 


««>-£ 


x(t),i 


dhi 

dhi 

dx\ 

dxn 

dhn 

dhn 

dx\ 

dxn 

(2.55) 


A  measurement  is  defined  as  the  sum  of  a  nominal  measurement  and  an  error: 


z(£i+i)  =  z(f  j_|_i)  +  Sz(ti+1) 


(2.56) 


Substituting  (2.54)  into  (2.56)  and  solving  for  the  measurement  error  provides 
the  error  update  equation: 


Sz(tf)  =  H(ti+i)6x(ti+1)  +  v(ti+ 1) 


(2.57) 


Finally,  the  post-measurement  error  state  and  covariance  are  calculated  using 
the  linear  Kalman  filter  update  equations. 
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The  Kalman  filter  is  unable  to  provide  any  benefit  to  a  system  without  measure¬ 
ments  of  the  surrounding  environment.  The  next  section  discusses  the  use  of  vision 
systems  to  provide  such  measurements. 

2.6  Vision- Aiding 

In  [20],  Veth  developed  an  image  and  inertial  fusion  navigation  algorithm.  The 
algorithm  fuses  image  and  inertial  data  by,  a)  collecting  an  image,  b )  transforming 
the  landmarks  in  the  image  to  feature  space,  c)  propagating  the  navigation  state  and 
feature  space  to  the  next  time  step,  d)  matching  features  from  the  propagated  feature 
space  to  the  new  feature  space,  and,  e)  estimating  the  landmark  location. 

2.6.1  Image  Collection.  Digital  imaging  devices  are  designed  to  measure  a 
pattern  of  light  intensities  projected  through  the  optics  and  aperture  onto  a  sensor. 
The  pattern  is  a  nonlinear  function  of  the  light  intensity,  optics,  and  pose  of  the  device 
relative  to  the  world.  These  devices  provide  a  three-dimensional  measurement  of  the 
world  corrupted  by  measurement  noise,  optical  distortions,  and  spatial  aliasing.  The 
three  dimensions  of  the  measurement  include  two  spatial  and  one  intensity  dimension. 

2.6.2  Transformation.  Computers  are  not  able  to  directly  distinguish  ob¬ 
jects  in  an  image,  therefore,  the  image  is  transformed  into  feature  vectors  residing  in 
feature  space,  using  an  algorithm  to  describe  the  pose  and  object  dimensions  of  the 
feature.  The  algorithm  chosen  for  the  transformation  is  a  variant  of  the  scale- invariant 
feature  tracking  (SIFT)  algorithm  [9]  as  the  stochastic  projection  can  be  applied  to 
the  pose  with  small  effects  to  the  object. 

The  feature  transformation  is  processed  by:  a)  computing  the  scale-space  de¬ 
composition,  b )  detecting  features,  and  c)  calculating  feature  description  vectors. 
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2.6.2. 1  Scale- Space  Decomposition.  The  scale-space  decomposition  is 


computed  using  a  Gaussian  spatial  filter  defined  by  the  following  function: 


g  [x,y,a 


(2.58) 


where  x  and  y  are  the  spatial  dimensions  of  the  image  in  pixels  and  a  is  the  standard 
deviation  of  the  blurring  function. 

The  difference  of  a  Gaussian  filter  is  defined  as: 


i(x,  y,  k,  a)  =  g(x,  y,  ha)  -  g(x,  y,  a) 


(2.59) 


where  k  >  1  is  the  scaling  frequency  step  constant. 

Given  an  initial  standard  deviation  (co),  and  a  scaling  frequency  step  constant 
(k)  the  ith  difference  of  a  Gaussian  filter  is: 


f  (x,  y ,  i)  =  g(x,  y,  kl+1a0 )  -  g(x,  y,  kla0 ) 


(2.60) 


Varying  the  scaling  frequency  step  constant  decomposes  the  image  into  multiple  scale 
spaces  centered  on  specific  spatial  frequencies.  The  step  constant  is  varied  in  a  manner 
to  maintain  equal  spacing  of  the  spatial  frequencies. 

When  scale-space  decomposition  of  the  image  is  complete  features  may  be  de¬ 
tected  readily. 


2. 6.2.2  Feature  Detection.  Candidate  features  are  detected  by  locating 


local  maxima  or  minima  within  the  spatial  and  scale  dimensions.  Then  the  spatial 
detail  for  each  candidate,  centered  on  the  candidate,  is  calculated  as  the  eigenvalues 
of  the  matrix: 


(2.61) 
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where  W  is  the  window  over  which  the  eigenvalues  are  computed,  and  Vfj,  and  Vfy 
are  the  gradients  of  f (x,y,i)  in  the  x  and  y  directions. 

The  strongest  candidates  are  chosen  as  features  by  thresholding  the  related 
eigenvalues  and  refining  with  the  metric: 

C(G)  =  (1  +  2kt)ai(J2  +  kt{(J  f  +  cr^)  (2.62) 


where  o\  and  <72  are  the  eigenvalues  of  G,  and  kt  is  a  scaling  parameter. 
The  pose  subspace  of  each  feature  is  constructed  as: 


z 


pose 

n 


( U ) 


Znx  (ti) 
Zny  (ti) 

&n(ti) 

On(ti ) 


(2.63) 


where  zUx,  zHy ,  crn  and  9n  are  the  pixel  location,  scale  and  primary  orientation  of 
feature  n  =  1...M  for  M  features  in  the  image  at  time  t%. 


2. 6. 2. 3  Feature  Description.  Finally,  with  the  features  within  an  im¬ 
age  detected,  the  object  dimensions  (feature  descriptions)  are  calculated  as  function  of 
the  intensity  gradient  of  the  scale-space  surrounding  each  feature.  The  object  dimen¬ 
sions  of  a  feature  are  composed  of  the  normalized  histogram  of  the  gradients  around 
the  feature. 

When  an  image  transformation  is  complete  the  image  is  represented  by  a  col¬ 
lection  of  M  vectors  in  feature  space: 


i(x,y,U)  ->  z*(t»)  Vn  e  {!,... ,Af} 


(2.64) 
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where 


<OSe{U)  4xl 
Z^'eCt(tl)l28xl 


(2.65) 


With  the  image  transformation  complete,  the  computed  features  are  now  prop¬ 
agated  to  the  next  time  step. 


2.6.3  Propagation.  The  state-space  error  estimate  and  landmark  error  are 
propagated  from  time  ti  to  ti+ 1  using  the  EKF  developed  in  Section  2.5.2. 

The  landmarks  detected  within  an  image  are  assumed  to  be  stationary,  How- 
ever,  a  small  additive  random  walk  is  added  to  account  for  the  effects  of  calibration 
and  initialization  errors,  and  to  prevent  the  covariance  from  collapsing  to  zero.  The 
landmark  error  is  defined  as: 


5yn(t)  =  Gy(t)wy(t)  (2.66) 

where  G y{t)  is  the  influence  matrix  and  w y{t)  is  zero-mean,  white  Gaussian  noise 
with  covariance: 

E  {w(t)wT(t  +  r)}  =  Q(t)6(r)  (2.67) 

The  navigation  and  landmark  error  covariances  are  propagated  with  the  follow- 


ing  equations: 

Pzx  W+l) 

=  $(ti+i,ti)Pxx(tt)$T(ti+i,ti) 

+  r+1  $(fm,r)G(r)Q(r)GT(r)$r(fm,r)dr 

Ju 

(2.68) 

P®3/(^i+ 1) 

=  ^(ti+^tOP^t^) 

(2.69) 

P yyV'i+i ) 

=  P yy(tf)  +  [ti+1  -  U]Gy(ti)Qy(ti)Gy  (ti) 

(2.70) 

2.6.4  Feature  Matching.  The  next  step  in  the  vision-aiding  algorithm  is 
to  statistically  match  the  predicted  features  with  a  new  set  of  measured  features  at 
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time  ti+i.  The  match  is  performed  by  defining  a  metric,  known  as  the  Mahalanobis 
distance,  to  measure  the  quality  of  a  match.  The  Mahalanobis  distance  is  defined  as 
the  weighted  inner  product  of  the  predicted  feature  vector,  z*(ti+i),  and  the  measured 
feature  vector,  z*(ti+1): 

Dniti+l)  =  [z*  (t  i+i)  -  Z*(ti+1)f  Pz*z,  (ti+ 1)  K(ti+ 1)  -  z*(ti+1)]  (2.71) 


where  Pz*z*{ti+\)  is  the  covariance  of  the  predicted  feature  vector.  Assuming  scale 
and  rotation  independence  of  the  pixel  location,  the  covariance  matrix  can  be  written 
as: 


'(U+i) 


^  zziti+l)  0  0  0 

0  Paa  0  0 

0  0  Pee  0 

0  0  0  P  zdzd 


(2.72) 


where  PZZ1  P CTCr,  Pgg,  and  P ZdZd  are  the  pixel  location,  scale,  rotation,  and  descriptor 
uncert  aint ies  resp  ect  i vely. 


As  the  SIFT  algorithm  provides  no  information  regarding  the  statistical  knowl¬ 
edge  of  P (j(j ,  Pee,  and  P ZdZd,  the  scale  and  orientation  are  given  zero  weight  and  the 
distance  metric  is  decomposed  into  pose  and  object  descriptor  distances: 


Dpn(ti+ 1)  [ZPn(A+ 1)  zp(^i+ 1)]  PzpZp  (ti+1  )  [ZPn  (^i+l)  Zp(ti+l)]  (2-71 3) 

Ddn(ti+ 1)  =  [zdn{ti+ 1)  —  Zd(t i+l)]  P ZdZd  (^*+l)  [zdn{ti+ 1)  —  zd(^i- |_i)]  (2.74) 


where 


,  (j'i+l) 


Pzz(U+i)  0  0  0 

0  oo  0  0 

0  0  oo  0 


P 


ZdZd 


(U+ 1) 


I 


(2.75) 

(2.76) 
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The  result  of  decomposing  the  distance  is  to  a)  include  only  the  effects  of  the  predicted 
pixel  location  in  the  pose  distance,  and  b)  weight  all  of  the  components  of  the  object 
descriptor  equally. 

The  two  distance  metrics  are  used  in  succession  to  improve  the  search  speed  and 
maintain  pixel  error  measurement  independence.  A  successful  match  is  found  when 
the  object  distance  is  below  a  pre-dehned  threshold.  Additionally,  the  application  of 
a  uniqueness  filter  is  available  to  reject  insufficiently  distinct  feature  matches. 

2.6.5  Landmark  Location  Estimation.  Landmark  location  estimates  are 
computed  using  using  binocular  stereopsis  as  it  does  not  require  a  terrain  model  and 
is  therefore  appropriate  for  navigation  in  unknown  environments.  For  convenience, 
a  binocular  reference  frame  Co  is  defined  midway  between  the  two  camera  frames  ca 
and  0,. 

The  computation  of  landmark  locations  is  completed  in  three  steps:  1)  feature 
selection  and  matching  2)  calculation  of  line  of  sight  from  the  binocular  origin  to  the 
landmark,  and  3)  estimating  the  landmark  location. 

2.6.5. 1  Feature  Selection  and  Matching.  The  selection  of  a  feature  is 
performed  on  one  of  the  images  of  the  binocular  pair  as  described  in  Section  2. 6. 2. 2. 

The  resulting  feature  is  then  statistically  projected  into  the  feature  space  of 
the  second  image.  For  example,  given  a  feature  in  the  ca-frame  the  projected  pixel 
location  in  the  Cb-frame  is  computed  as: 

zb  =  Tpix^cb  (2.77) 

s»  =  C»  [*C2T>“  +  p«m„  -  p»  J  (2.78) 

where  T^'-x  and  Tf*x  are  the  camera  projection  matrices  for  cameras  a  and  b ,  p c°arria 
and  Pc°amb  are  location  vectors  of  the  camera  frames  relative  to  the  binocular 
frame,  and  k  is  the  unknown  distance  parameter. 


27 


Variations  in  the  distance  parameter  describe  an  epipolar  line  in  the  cb-frame 


image  modeled  as  a  Gaussian  distribution  with  mean  and  variance: 

k  =  E[k\  =  4.5  dbinoc  (2.79) 

al  =  E  [(k  -  k )2]  =  9  4noc  (2.80) 

where  the  binocular  disparity  dbinoc  is  dehned  as: 

dbinoc  =  \\pCCama  ~  Pcamb\\  (2'81) 

Matching  of  features  is  performed  by  determining  the  features  within  a  statis¬ 
tical  distance  of  the  prediction  and  choosing  the  feature  with  the  smallest  feature 
description  distance.  The  method  for  matching  features  is  the  same  a  discussed  in 
Section  2.6.4. 

2. 6. 5. 2  Line  of  Sight.  Next,  the  estimate  of  the  relative  line  of  sight 
from  the  Cq- frame  origin  to  the  landmark  is  computed  from  the  pixel  locations  of  the 
landmark  in  each  camera  frame: 

z“  =  K  -  pSraJ  (2.82) 

=  Tg'CS  Is?  -  PoU  (2.83) 

The  estimation  results  in  an  estimated  line  of  sight  vector,  Sq°,  and  corresponding 
covariance,  PSoS0. 

2. 6. 5. 3  Location  Estimation.  Finally,  the  n- frame  landmark  location 
is  estimated  by  substituting  the  line  of  sight  vector  estimate  and  navigation  state 
estimate  into  the  landmark  location  equation: 

y"  =  P"  +  C£  [pj  +  C^sf]  (2.84) 
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In  the  next  section,  the  concepts  of  federated  filtering  are  discussed,  providing 
the  architecture  upon  which  the  sharing  of  information,  and  navigation  state  estimates 
are  developed. 


2.1  Federated  Filtering 

The  federated  filter,  as  described  in  [2],  is  an  information-sharing  filter,  there¬ 
fore,  before  the  federated  filter  may  be  described  the  term  information  must  be  de¬ 
fined.  The  following  definition  of  information ,  from  [2],  is  used  throughout  this  thesis. 


Information  within  an  optimal  estimator  (filter)  generally  represents  knowl¬ 
edge  of  the  random  process  (state)  being  estimated.  Information  is  gained 
from  initial  state  estimates  and  periodic  measurements.  Information  is 
lost  through  propagation  uncertainties  (processed  noise).  Specifically ,  in¬ 
formation  within  a  filter  is  represented  by  its  information  matrix  and  its 
information  state  vector  (or  equivalently,  by  its  error  covariance  matrix 
and  standard  state  vector).  The  information  matrix  (covariance  matrix 
inverse)  is  a  statistical  measure  of  the  amount  of  information  present.  The 
information  state  vector  (product  of  the  information  matrix  and  standard 
state  vector)  represents  specific  knowledge  of  the  actual  random  process. 
Greater  information  implies  smaller  estimation  errors,  and  vice  versa. 

The  federated  filter  is  designed  to  share  information  amongst  multiple  local 
filters  and  one  master  filter,  and  from  their  solutions  build  a  total  solution.  The  basic 
information  sharing  methodology  implemented  by  the  federated  filter  is: 


•  divide  the  total  system  information  among  several  component  local  filters  and 
a  single  master  filter; 

•  perform  local  and  master  filter  time  propagation  and  measurement  processing, 
adding  local  sensor  information; 

•  recombine  the  updated  local  and  master  filter  information  in  to  a  new  total 
sum. 

A  typical  federated  filter  set  up  is  depicted  in  Figure  2.2.  Local  sensors,  l...n, 
each  have  an  associated  local  filter  with  inputs  from  the  local  sensor  and  a  common 
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Figure  2.2:  Information  Partitioning  in  Federated  Filter  [2], 

reference  INS.  Additional  local  sensor  measurements  without  a  local  filter  are  provided 
directly  to  the  master  filter  along  with  the  solutions  of  the  local  filters. 

The  equivalent  centralized  filter  solution  is  represented  by  the  covariance  matrix 
P  and  state  vector  x.  Similarly,  the  local  filter  solutions  are  represented  by  PZ  and 
xZ;  and  the  master  filter  by  Pm  and  xm.  The  index  i  —  1 . . .  n  is  used  to  index  the 
local  filters  alone,  while  the  index  k  —  1 . . .  n,  m  is  used  to  index  the  local  filters  and 
a  master  filter,  where  k  =  m  represents  the  master  filter. 
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2.7.1  Construction.  The  formulation  in  this  section  is  derived  from  [2] .  The 
federated  filter  is  constructed  such  that  statistically  independent  local  and  master 
filter  solutions  can  be  combined  at  any  time  with  the  following  additive  information 
algorithm, 


P  1  =  Pm  1  +  PI  1  +  . . .  +  Pn  1  (2.85) 

P-'x  =  Pm_1xm  +  Pl-1xl  +  . . .  +  Pn_1xn  (2.86) 

where  the  inverse  covariance  matrix  (P"1)  is  known  as  the  “information  matrix”. 

Proper  construction  of  the  federated  filter  is  accomplished  by  applying  a  fraction 
of  the  total  information  to  each  local  filter  and  the  master  filter, 

PA;"1  =  P"1  (3k  or  P  =  P  fa 1  (2.87) 

while  maintaining  constant  total  information  across  the  sum  to  satisfy  the  conserva¬ 
tion  of  information  principle.  Therefore,  the  share-fraction  values  (/?&)  must  sum  to 
unity: 

n,m  n 

X>  =  /^  +  X>  =  1  (2-88) 

k= 1  i= 1 

The  share-fraction  values  ((3k)  are  only  applied  to  the  information,  therefore, 
the  local  and  master  filter  state  vectors  are  equal  to  the  total  state  vector. 

xA;  =  x  (2.89) 

Application  of  the  local  and  master  filter  solutions  given  by  (2.87)  and  (2.89) 
to  the  additive  information  algorithm  (2.85)  demonstrates  the  local  and  master  filter 
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solutions  can  be  combined  to  yield  the  correct  total  solution.  (P,  x). 


Pm  1  +  PI  1  +  . . .  +  Pn  1 

(2.90) 

P  1  pm  +  P"1  fit  +  . . .  +  P-1  I3n 

(2.91) 

n,m 

p -'V& 

(2.92) 

k= 1 

P  1 

(2.93) 

P m_1xm  +  Pl_1xl  +  . . .  +  P n_1xn 

(2.94) 

P_1  /3mx  +  P"1  p1  x  +  . . .  +  P_1  /3nx 

(2.95) 

n,m 

(2.96) 

k=  l 

P  xx 

(2.97) 

When  the  common  process  noise  information  is  divided  in  the  same  fashion,  the 
discrete  time  propagation  process  can  be  performed  by  independent,  parallel  opera¬ 
tions  of  the  local  and  master  filters. 

2.1.2  Propagation.  The  local  and  master  filters  are  propagated,  in  parallel, 
provided  the  common  process  noise  information  is  divided  in  the  same  fashion  as  the 
information  matrix.  The  covariance  propagation  equations  from  time  t  —  1  to  t  are: 

Pkt  =  &kt  Pfc(t_i)  +  G kt  Q kt  G kf  (2.98) 

Assuming  the  local  and  master  filters  are  full-sized,  the  transition  matrices  <frk 
equal  <I>.  and  the  noise  distribution  matrices  G k  equal  G.  However,  the  process  noise 
covariance  matrices  Q k  are  governed  by  the  information-sharing  principle. 

Q k~l  =  Q1  /3k  or  Q  =  Q  (2.99) 
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The  post-propagation  information  matrix  can  be  obtained  through  the  additive 
information  algorithm,  when  the  information  matrices  are  obtained  through  (2.87), 
and  the  noise  information  matrices  are  obtained  through  (2.99). 


n,m 


Y,Pk~1  =  Y.  [*!  P«-1>  /»_1  * r  +  G<  Q(  /3V1  Gf] 


k= 1 


k 

~n,m 


E^ 


_k=  1 

=  P  1 


[*t  P(t-D  +  G,  Qt  Gf] 


-i 


(2.100) 

(2.101) 

(2.102) 


2.7.3  Measurement  Update.  After  propagation  each  local  filter  incorporates 
discrete  measurements  z i  from  its  sensor.  Measurement  information  is  added  to  local 
filter  i  through, 


p?;1  =  pr1  +  Hi  Rr1  HiT 

P xi+  =  Pi-1  xi  +  Hi  Ri"1  zi 

(2.103) 

(2.104) 

where  the  subscript  +  refers  to  posts  measurement  values,  Ri"1 
measurement  information  matrix  and  Hi  =  (5z i/hxi)T. 

is  the  local  filter 

Using  the  additive  information  algorithm  to  combine  the  results  of  local  filter 

measurement  updates  provides  the  correct  total  solution, 

n 

Pm"1  +  Y  [P*"1  +  Hi  R*_1  HzT] 

2=1 

(2.105) 

n 

=  p-1  +  y  m  Ri_1  mT 

2=1 

(2.106) 

+  i 

(2.107) 
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and, 


n 


Pm  1  xm  +  Y.\p  i  1  xi  +  Hi  R?  1  z  i 


(2.108) 


i= 1 


n 


=  P  1  x  +  V  Hi  Ri  1  zi 


(2.109) 


i= 1 


(2.110) 


Thus  the  federated  filter  provides  a  solution  which  would  be  achieved  by  a 
single  filter  processing  all  of  the  sensor  measurement  sets,  ffowever,  the  federated 
filter  implementation  of  information  sharing  provides  three  advantages  over  a  single 
filter  implementation  [2]: 

•  increased  measurement  data  throughput  by  parallel  operation  of  local  filters, 
and  by  data  compression  within  local  filters; 

•  enhanced  system  fault-tolerance  by  maintaining  multiple  component  solutions 
to  improve  fault  detection  and  recovery  capabilities;  and 

•  improved  accuracy  and  stability  of  cascaded  filter  operations,  via  use  of  theo¬ 
retically  correct  estimation  algorithms. 

2.7.4  Filter  Reset.  Finally,  the  federated  filter  may  be  implemented  using 
one  of  four  different  local  filter  reset  architectures  [3].  The  four  architectures,  also 
know  as  reset  modes,  are  1 )  no-reset,  2)  fusion-reset,  3)  zero-reset,  and  4)  rescale. 

In  the  no-reset  mode,  the  local  filters  operate  autonomously  while  maintaining 
the  long-term  information  of  the  navigation  system.  The  master  filter  fuses  the  local 
filter  solutions  at  each  time  step  and  propagates  the  fused  solution  to  the  next  time 
step.  The  master  filter  discards  the  propagated  solution  prior  to  fusing  the  next  set 
of  local  filter  solutions. 

The  local  filters  retain  the  long-term  system  information  in  the  fusion-reset 
mode,  but  do  not  operate  autonomously  as  in  the  no-reset  mode.  In  this  mode  the 
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master  filter  divides  the  fused  information  and  applies  an  equal  share  to  each  of  the 
local  filters.  Each  local  filter  begins  the  next  cycle  with  the  same  information  as  the 
other  filters.  Therefore,  the  weaker  local  filters  gain  information  from  the  stronger 
filters,  which  may  help  constrain  the  weaker  filters. 

In  the  zero-reset  mode  the  long-term  system  information  storage  is  transfered  to 
the  master  filter.  Each  local  filter  acts  as  a  integrate-and-dump  filter  compressing  it’s 
sensor  measurements  into  a  single  solution.  After  the  local  filters  send  their  solutions 
to  the  master  filter,  they  reset  themselves  to  zero  information.  This  ensures  the  next 
set  of  inputs  contain  no  old  information. 

The  rescale  mode  is  similar  to  the  zero-reset  mode,  except  the  local  filters  send 
half  of  their  information  to  the  master  filter.  Additionally,  the  local  filters  reset  their 
information  to  one-half  instead  of  zero  after  sending  the  solution.  This  mode  allows 
the  local  hlters  to  retain  enough  information  to  make  the  filter  locally  usable  in  the 
event  of  failure. 

2.8  Multiple  Vehicles 

In  this  section,  two  approaches  to  cooperative  multiple  platform  navigation  are 
discussed  and  analyzed.  Additional  approaches  to  multiple  platform  navigation  may 
be  found  in  [1,8,11,16,17]. 

2.8.1  Sanderson.  In  [18]  Sanderson  proposes  a  CNS  based  on  the  appli¬ 
cation  of  inter-platform  range  measurements  to  a  linear  Kalman  filter.  The  use  of 
such  range  measurements  requires  each  platform  to  estimate  the  position  of  all  other 
platforms  to  which  it  performs  a  range  measurement.  This  poses  a  problem  during 
the  propagation  step  of  the  Kalman  filter  as  each  platform  does  not  have  implicit 
knowledge  of  the  trajectory  of  the  other  platforms.  Sanderson  mitigates  this  problem 
by  pre-programming  each  platform  with  a  trajectory  consisting  of  a  series  of  desire 
positions.  At  each  time  step  the  platforms  transmit  their  next  desired  position  so  the 
other  platforms  may  properly  propagate  the  position  estimate. 
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Sanderson  demonstrates  the  the  validity  of  the  CNS  with  three,  10  run,  ex¬ 
periments  using  two  robots  traveling  in  parallel  along  a  10  m  long  straight  path.  In 
the  first  experiment  to  two  robots  do  not  cooperate,  resulting  in  an  average  error  of 
approximately  10  cm  at  the  end  of  the  10  m  run.  The  second  experiment  is  performed 
with  the  robots  cooperating,  resulting  in  an  average  error  less  than  10  cm  at  the  end 
of  the  run.  In  both  of  these  cases  the  position  estimate  is  diverging  from  truth  at 
the  end  of  the  run.  Finally,  the  third  experiment  uses  the  CNS  with  the  addition  of 
one  of  the  robots  following  a  wall  at  a  set  distance.  This  results  in  a  less  than  1cm 
average  error  for  the  robot  which  is  not  following  the  wall. 

The  CNS  developed  by  Sanderson  was  created  for  the  navigation  of  platforms 
constrained  to  a  two-dimensional  surface.  However,  the  concepts  used  in  the  CNS 
development  may  be  extrapolated  to  three  dimensions.  A  major  limiting  factor  in  the 
use  of  this  system  for  large  numbers  of  platforms  is  the  requirement  for  each  platform 
to  estimate  the  other  platforms  positions.  This  requirement  increases  the  number 
of  states  in  the  Kalman  filter  and  will  therefore  increase  the  processing  time  of  the 
system  with  increased  numbers  of  platforms. 

2.8.2  Panzieri,  Pascucci  and  Setola.  In  [15]  Panzieri,  Pascucci  and  Setola 
circumvent  the  problems  with  large  state  vectors,  due  to  the  number  of  platforms 
in  the  network,  by  implementing  a  CNS  using  an  interlaced  Kalman  filter.  Using 
this  method  each  platform  only  estimates  its  own  pose  and  shares  estimated  posi¬ 
tions  and  covariances  when  in  communication  range  of  other  platforms.  Additionally, 
when  platforms  are  close  enough,  range  measurements  may  be  incorporated  into  the 
solution. 

The  CNS  is  demonstrated  using  five  Matlab®  simulations.  The  simulations 
consist  of  10  platforms  operating  within  a  12  x  12  arena  which  includes  nine  landmark 
beacons.  The  five  simulations  progress  from  a  complete  non-cooperative  system  to 
a  full  system  capable  of  communication  between  platforms  and  all  of  the  landmark 


36 


beacons.  Over  the  simulations  the  mean  position  error  of  the  10  platforms  is  reduced 
from  2  m,  for  the  non-cooperative  system,  to  less  than  2  cm  for  the  full  CNS. 

The  CNS  system  developed  by  Panzieri,  Pascucci  and  Setola  demonstrates  the 
ability  of  a  interlaced  Kalman  filter  approach  to  reduce  the  state  vector  of  a  platform, 
increasing  the  rate  at  which  the  solution  is  computed. 

In  both  approaches  to  the  CNS  developed  utilize  only  information  pertaining  to 
the  condition  of  the  platforms.  In  the  next  chapter,  vision- aiding  of  an  INS  is  extended 
to  use  with  multiple  platforms  cooperating  to  improve  the  navigation  solution  of  the 
group. 
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III.  Methodology 


This  chapter  fully  develops  two  methods  for  the  implementation  of  the  multiple 
vehicle,  cooperative,  vision-aided  navigation  system.  The  chapter  begins  with 
an  overview  of  the  multiple  platform  federated  filter  and  describes  the  differences 
between  the  two  methods  of  implementation.  Next,  a  new  approach  of  local  filter 
scaling  is  discussed  to  simplify  the  development  of  the  primary  local  filter  implemented 
in  the  first  method.  Then,  the  primary  local  filter  is  defined  followed  by  descriptions 
of  the  landmark  extraction  and  matching  algorithms.  Next,  the  range  measurement 
algorithm  is  developed  followed  by  the  formation  of  the  secondary  local  filter  function. 
Finally,  the  fusion  algorithm  which  comprises  the  master  filter  is  developed. 

3.1  Multiple  Platform  Federated  Filter 

The  federated  filter  as  developed  in  Section  2.7  is  designed  for  the  express  pur¬ 
pose  of  fusing  navigation  solutions  from  multiple  local-level  filters  on  a  single  platform 
utilizing  multiple  sensor  measurements.  The  federated  filters  developed  for  this  re¬ 
search  operate  in  the  same  manner,  differing  only  in  the  source  of  sensor  measurements 
for  the  secondary  local  filter. 

The  first  method  of  implementation,  designated  the  “post-scaling”  method  and 
shown  in  Figure  3.1,  is  developed  to  utilize  the  algorithms  developed  by  Veth,  as 
detailed  in  Section  2.6,  with  minimal  modification.  The  objective  of  this  method 
is  to  implement  these  algorithms  in  the  primary  local  filter  with  no  modifications. 
As  the  covariance  matrix  is  pre-scaled  in  a  federated  filter,  this  method  requires  the 
development  of  a  new  way  to  scale  the  covariance  matrix,  after  the  propagation  and 
measurement  update  of  the  Kalman  filter. 

The  second  method  of  implementation,  designated  the  “pre-scaling”  method  and 
shown  in  Figure  3.2,  modifies  the  algorithms  developed  by  Veth.  These  modifications 
allow  for  the  pre-scaling  of  the  covariance  matrix,  and  operation  of  the  filter  for  a 
single  time  step. 
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Figure  3.1:  Platform  Block  Diagram  (Method  1):  The  primary  local  filter  (green) 
receives  measurements  from  on-board  sensors.  The  secondary  local  local  filter  (yellow) 
receives  measurements  from  remote  platforms  through  the  extraction  and  matching 
functions.  The  master  filter  (blue)  scales  the  outputs  and  fuses  the  results  of  the  two 
local  filters  to  provide  a  total  system  solution. 

The  basic  structure  of  the  navigation  system  is  the  same  regardless  of  the  method 
of  implementation.  The  system  consists  of  a  primary  and  secondary  local  filter,  a 
landmark  extraction  algorithm,  a  landmark  matching  algorithm,  and  a  master  filter. 

The  most  significant  difference  between  the  two  methods  of  implementation  is 
when  the  local  filter  solutions  are  scaled.  The  post-scaling  method,  shown  in  Fig¬ 
ure  3.1,  performs  local  filter  scaling  in  the  master  filter  prior  to  fusion  of  the  local 
solutions.  In  Figure  3.2  the  scaling  block  is  removed  as  the  pre-scaling  method  scales 
the  local  filters  during  the  initialization  of  the  primary  filter. 
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Figure  3.2:  Platform  Block  Diagram  (Method  2):  The  primary  local  filter  (green) 
receives  measurements  from  on-board  sensors.  The  secondary  local  local  filter  (yellow) 
receives  measurements  from  remote  platforms  through  the  extraction  and  matching 
functions.  The  master  filter  (blue)  fuse  the  results  of  the  two  local  filters  to  provide 
a  total  system  solution. 


In  the  next  section,  the  new  local  scaling  method  is  developed  for  the  imple¬ 
mentation  of  the  post-scaling  navigation  system  method. 


3.2  Local  Filter  Scaling 

To  utilize  the  navigation  filter  developed  by  Veth,  without  major  modifications 
to  scale  the  covariance  matrices,  a  new  scaling  method  is  developed.  The  formation  of 
this  new  scaling  method  begins  with  the  definitions  of  the  local  covariance  matrices, 
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at  any  time  t,  provided  in  [3], 


p  k  = 

PCt  <7 pci 

0 

0 

P  Bt  7 pbi 

Q  it  = 

Q  C't  ^ pci 

0 

0 

Q  Bt  7 pbi 

Pit  = 

1 

£ 

0 

0 

R Pt  ''I phi 

(3.1a) 

(3.1b) 

(3.1c) 


where  (Pit,  Q It,  Pit)  are  the  covariance  matrices  of  the  local  filter,  (PC*,  QCt,  R(7f) 
are  the  portions  of  the  full  system  covariance  matrices  common  to  all  of  the  local  filters, 
(P Bt,  Q Bt,  R Bt)  are  the  portions  of  the  full  system  covariance  matrices  unique  to  the 
local  filter,  and  (7 pci,  7 pbi )  are  the  federated  filter,  covariance  mode,  scaling  parameters. 

The  full  state  estimate  vector  is  divided  into  two  categories:  common  states, 
and  unique  states.  Common  states  are  those  states  estimated  by  all  local  filters,  while 
unique  states  are  estimated  only  by  the  local  filter  which  performs  a  measurement  of 
the  state. 

The  primary  and  secondary  local  filters  in  Figure  3.1  share  all  of  the  system 
states.  With  no  unique  states  in  either  of  the  local  filters,  the  scaling  parameters 
are  equal,  and  7 pi  =  7 pci  =  7^.  The  scaling  parameter  (7 pi)  is  extracted  from  the 
matrices  and  the  local  filter  covariances  become: 


Pit  =  'fpl  p t 

(3.2a) 

Qh  7 pi  Q t 

(3.2b) 

Pk  7 pi  R  / 

(3.2c) 

The  two  local  filters  share  all  of  the  states  for  which  measurements  are  available, 
therefore,  the  measurement  noise  covariance  (Rj)  must  be  scaled  as  well  as  the  process 
(Pt)  and  process  noise  (Qt)  covariances. 
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The  local  filter  state  vector  and  process  covariance  are  initialized,  in  covariance 
mode,  as  defined  in  [3]. 


x!,+  =  x+  (3.3) 

Pi,+  =  %i  P t  (3.4) 

where  t  is  equal  to  zero  for  initialization. 

As  shown  in  [2]  the  state  and  noise  transformation  matrices  do  not  require 
scaling. 


Git  =  Gt 

The  states  are  propagated  as  follows: 

x/“  =  <S>lt  x/+_1} 

Substituting  (3.3)  and  (3.5)  into  (3.7)  then  solving  for  x/^  provides, 


Next,  the  covariance  matrix  is  propagated, 

Pf,“  =  'I'/,  PiJ.j,  +  Gi,  Qi,  Gif 


(3.5) 

(3.6) 


(3.7) 


(3.8) 

(3.9) 


(3.10) 
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Substituting  (3.2a),  (3.2b)  and  (3.6)  into  (3.10)  then  solving  for  P lt  provides, 


Ipi  P(i_i)  +  Gt  t pi  Qt  Gj 

(3.11) 

lpl  ($i  P+_1}  +  Gi  Qi  Gj) 

(3.12) 

Ipi  pt 

(3.13) 

Therefore,  the  state  vector  and  process  covariance  may  be  propagated  from 
one  time  step  to  the  next,  and  the  scaling  factor  applied  after  the  propagation  is 
complete.  However,  the  new  scaling  method  must  continue  through  the  measurement 
update  process  to  be  useful. 

As  demonstrated  in  [2]  the  measurement  transformation  matrix  is  not  scaled. 

Hit  =  Ht  (3.14) 

With  the  measurement  transformation  matrix  (3.14)  and  process  covariance 


matrix  (3.11)  defined,  the  Kalman  gain  is  computed, 

K lt  =  P l~  mj  (H It  P /-  UlJ  +  R/i)'1  (3.15) 

Substituting  (3.2c),  (3.11)  and  (3.14)  into  (3.15)  then  solving  for  K lt  provides, 

K l,  =  7P(  P,-  Hf  (H,  Tpl  P-  Hj  +  R,)"1  (3.16) 

=  7pi  P,“  Hf  —  (H,  P,-  Hj  +  R,)  (3.17) 

'Ipl 

=  (Hi  Pb  Htkr  +  Rf)_1  (3.18) 

=  Ki  (3.19) 


This  computation  of  the  Kalman  gain  is  only  valid  if  the  measurement  noise 
covariance  (Rf  )  is  scaled  identically  to  the  process  (Pi)  and  process  noise  (Qt)  covari- 
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ances.  Otherwise,  the  scaling  parameters  will  not  cancel  and  the  local  filter  Kalman 
gain  requires  pre-scaling. 

As  with  the  state  vector  and  measurement  transformation  matrix,  the  measure¬ 
ment  vector  does  not  require  scaling. 


z  lt  =  z  t  (3.20) 

The  measurement  update  of  the  state  is: 

ydt  =  x^-  +  K lt  (z lt  —  HlJ  x/t_)  (3-21) 

Substituting  (3.8),  (3.16),  (3.20)  and  (3.14)  into  (3.21)  then  solving  for  x/+ 
provides, 

x4+  =  XT  +  K,  (z t  -  x" )  (3.22) 

=  x+  (3.23) 


Finally,  the  process  covariance  is  updated, 

P Z+  =  P l~  -  K lt  H P l~  (3.24) 

Substituting  (3.11),  (3.16),  and  (3.14)  into  (3.24)  then  solving  for  P/t+  provides, 

P K  =  Ipi  P*  —  K-t  Ht  7 pi  Pt  (3.25) 

=  7p!  (Pr  -  K,  P,-)  (3.26) 

=  7p!  P,+  (3.27) 


Therefore,  the  measurement  update  of  the  state  vector  and  process  covariance 
does  not  require  pre-scaling  of  the  process  covariance.  This  result  allows  for  a  set  of 
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local  filters  to  run  independently  without  scaling  the  information  applied  to  them. 
Samples  are  taken  from  the  local  filters  as  needed  and  scaled  prior  to  fusion  by  the 
master  filter. 

3.3  Primary  Local  Filter 

The  primary  local  filter  implements  the  navigation  system  developed  by  Veth, 
as  described  in  Section  2.6.  Measurements  to  the  filter  are  provided  by  the  onboard 
vision  system. 

3.3.1  Post-Scaling.  The  only  modification  to  the  algorithm,  to  implement 
the  post-scaling  method,  is  the  addition  of  a  single  function  to  extract  the  initial 
covariance  related  to  a  landmark  when  a  new  landmark  is  added  to  the  navigation 
state.  This  function  is  applied  to  both  methods  and  is  detailed  in  Section  3.3.3.  The 
resulting  state  and  process  covariance  of  the  primary  filter  are  scaled  in  the  master 
filter  prior  to  fusion. 

3.3.2  Pre-Scaling.  The  pre-scaling  method  makes  three  modifications  to 
the  original  Veth  algorithm.  First,  the  initial  covariance  related  to  a  landmark  is 
extracted  in  the  same  manner  as  in  the  post-scaling  method.  Next,  the  process  (P) 
and  processed  noise  (Q)  covariances  are  scaled  by  the  scaling  factor  (7).  The  scaling 
factor  is  2  when  the  navigation  system  is  run  in  no-reset  mode  and  3  in  fusion-reset 
mode,  as  described  in  Section  2.7.4. 

The  final  modification  to  the  Veth  implementation  is  the  removal  of  time  loop¬ 
ing.  The  original  code  processes  the  entire  simulation  in  one  call  of  the  code  and  then 
provides  results  for  every  time  step.  The  modification  removes  the  loop  to  provide 
a  result  for  a  single  time  step.  This  modification  allows  for  the  pre-scaling  of  the 
covariances  and  operation  of  the  navigation  system  in  one  of  the  reset  modes. 

3.3.3  Initial  Covariance  Extraction.  Immediately  after  a  landmark  is  added 
to  the  primary  filter,  the  initial  covariance  extraction  function  creates  and  saves  two 
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matrices.  The  first  matrix  (M)  is  a  mask  of  the  process  covariance  matrix  and  consists 
of  zeros  in  the  three  columns  and  rows  pertaining  to  the  added  landmark,  and  ones 
elsewhere.  The  second  matrix  (L)  is  a  copy  of  the  process  covariance  matrix  with 
all  values  outside  of  the  three  columns  and  rows  pertaining  to  the  added  landmark 
set  to  zero.  These  two  matrices  are  computed  once  for  every  landmark  added  to 
the  primary  filter,  and  are  provided  to  the  secondary  filter  to  facilitate  resetting  the 
process  covariance  when  a  new  landmark  match  is  made. 

The  output  of  the  primary  filter  at  each  time  step  is  now  applied  to  the  master 
filter  and  landmark  extraction  function. 

3.4  Landmark  Extraction 

The  landmark  extraction  function  computes  the  error  corrected  n-frame  posi¬ 
tion  of  the  platform  and  all  landmarks  tracked  by  the  primary  filter.  The  function 
then  extracts  the  platform  position,  landmark  positions,  and  all  related  variances. 
Additionally,  the  token,  scale,  and  rotation  data  for  each  landmark  is  extracted  to 
facilitate  landmark  matching. 

The  first  step  in  the  function  is  to  correct  the  position  of  the  platform  and 
landmarks  by  subtracting  the  pertinent  error  state  from  the  estimated  platform  and 
landmark  position  estimates: 


pn  =  pn  -  pn 


Yk  =  yk~  yl? 


(3.28) 

(3.29) 


where  pn  is  the  corrected  platform  position,  and  y£  is  the  corrected  landmark  position 
for  the  kth  landmark. 


Next  the  covariance  of  the  platform  position  is  extracted  from  the  full  navigation 
state  covariance  matrix. 


1 1/  ■/ '  P  pp  PpxPxa;Ppx 


(3.30) 
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where  Hpp  is  the  resulting  covariance,  Ppp  is  the  portion  of  the  state  covariance  related 
to  the  platform  position,  Pxx  is  the  portion  of  the  state  covariance  related  to  all  other 
states,  and  Ppx  is  the  portion  of  the  state  covariance  relating  the  position  to  all  of 
the  other  states.  This  result  provides  the  platform  position  covariance  including  the 
effects  of  all  other  states  and  their  related  measurements.  The  platform  position  and 
covariance  are  extracted  for  use  in  the  range  measurements  as  detailed  in  Section  3.6. 

Finally,  the  variances  of  the  landmarks  are  extracted.  The  extraction  function 
used  for  the  platform  position  is  not  valid  for  the  landmarks  as  it  will  include  the 
information  about  the  platform  position.  This  information  will  cause  a  bias  when  the 
result  is  applied  to  a  second  platform.  Therefore,  the  variance  for  each  landmark  is: 

RykVk  =  ^a9(PykVk)  (3.31) 

where  k  —  1 . . .  n  is  the  kth  landmark,  P,ykVk  is  the  resulting  variance,  and  PykVk  is 
the  3x3  portion  of  the  process  covariance  related  to  the  landmark.  Extraction  of 
the  landmark  variances  by  this  method  is  not  optimal  as  information  is  lost  in  the 
extraction.  However,  if  the  information  is  extracted  in  an  optimal  manner  and  shared 
with  other  platforms,  a  bias  is  induced  in  the  position  estimate  due  to  the  difference 
in  position  of  the  platforms. 

After  all  required  data  is  extracted  from  the  primary  filter  the  result  is  trans¬ 
ferred,  through  communications  channels,  to  all  other  platforms  participating  in  the 
cooperative  system. 

The  next  section  details  the  target  matching  function  using  the  extracted  data 
from  other  platforms. 

3. 5  Target  Matching 

The  landmark  matching  function  provides  the  secondary  local  filter  with  mea¬ 
surements  of  landmark  positions  provided  by  other  platforms  connected  to  the  coop¬ 
erative  navigation  system.  The  landmarks  of  interest  are  limited  to  those  tracked  by 
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the  primary  local  filter.  Therefore,  this  function  compares  the  landmarks  sent  from 
other  platforms  to  those  tracked  by  the  primary  filter  and  discards  any  which  do  not 
match. 

A  match  correlation  value  {A]k)  is  computed  for  every  possible  combination  of 
primary  filter  landmark  (j)  and  cooperative  platform  landmark  (k).  This  value  is 
computed  as  the  arc  cosine  of  the  dot  product  of  the  SIFT  tokens  for  each  primary 
filter  and  cooperative  platform  landmark  combination: 


Ajk  =  arccos(Tj  •  Tk) 


(3.32) 


where  T  is  the  SIFT  token  of  the  respective  landmarks. 

A  landmark  k  is  considered  a  match  to  landmark  j  if  all  of  the  following  three 
conditions  apply: 

1.  the  correlation  for  landmark  k  is  the  smallest  of  all  correlations  computed  for 
landmark  j, 

2.  the  correlation  for  landmark  k  is  larger  than  the  sift  minimum  correlation  value, 
and 

3.  the  correlation  for  landmark  k  is  smaller  than  the  second  smallest  correlation 
multiplied  by  the  sift  feature  distinction  value. 

When  landmark  matching  his  complete  the  results  are  applied  as  measurements 
to  the  secondary  local  filter  along  with  range  measurements  when  available. 

3. 6  Range  Measurements 

In  addition  to  landmark  measurements,  the  secondary  local  filter  may  receive 
range  measurements  to  other  platforms  cooperating  in  the  navigation  system.  A 
measurement  update  requires  three  variables;  the  measurement,  an  error  state  to 
measurement  state  transformation  matrix,  and  measurement  covariance.  The  mea¬ 
surement  is  the  direct  output  of  the  measurement  device,  while  the  transformation 
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matrix  and  covariance  are  calculated  from  the  position  and  covariance  of  the  two 
platforms  involved. 

A  measurement  of  the  range  between  two  platforms  (z),  regardless  of  the  mea¬ 
surement  device,  is  the  root-sum-squared  (RSS)  difference  in  position  (p)  of  the  two 
platforms  plus  a  measurement  error  (u): 

Z  =  |  Apafe|  +V 
=  |  Pa  -  P&|  +V 

and  may  be  expressed  as  an  error  measurement  by: 


Sz  = 

^Pa 

Haa  H ab 

$Pb 

=  Haa<5pa  +  Hab5pb  +  v  (3.35) 

where  a  represents  the  platform  performing  the  measurement,  b  represents  the  plat¬ 
form  to  which  the  measurement  is  made,  H aa  is  the  transformation  matrix  from  the 
state  space  of  platform  a  to  the  measurement  space  of  platform  a,  and  Hab  is  the 
transformation  matrix  from  the  state  space  of  platform  b  to  the  measurement  space 
of  platform  a. 

The  measurement  and  remote  platforms  each  provide  an  estimate  of  their  re¬ 
spective  position.  When  these  estimates  are  applied  to  equation  (3.34), 

z  =  \pna-pnb\  +  v  (3.36) 

=  y/{xa  +  Xb)2  +  (ya  +  Vb)2  +  (za  +  Zb)2  +  v  (3.37) 


(3.33) 

(3.34) 
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the  range  measurement  is  equal  to  the  estimated  range  plus  the  measurement  error. 
Therefore,  the  estimated  range  is: 


£  =  \J  (xa  +  Xb )2  +  (ya  +  yb )2  +  (za  +  zb )2  (3.38) 


The  error  to  measurement  state  transformation  matrices  are  calculated  from  the 
partial  derivatives  of  the  range  estimate  with  respect  to  the  position  coordinates  of 
each  platform. 


Haa  = 
H  ab  = 


dz  dz  dz 
dxa  dya  dza 
dz  dz  dz 
dxb  dyb  dzb_ 


(3.39) 

(3.40) 


The  partial  derivative  of  the  range  estimate  with  respect  to  the  x  coordinate  of 
platform  a  is  the  difference  between  the  two  platform  x  coordinates  divided  by  the 
range  estimate; 


dz 


dx„ 


xa  -  xb 


Xa  -  Xb 


\J  (xa  +  Xb )2  +  (ya  +  yb )2  +  (za  +  zb )2 


(3.41) 


and  the  calculations  for  the  remaining  partial  derivatives  for  platform  a  are  similar; 


dz 

dya 

dz 

dza 


x=x 


x=x 


Va  yb 

z 

Za  Zb 
Z 


(3.42a) 

(3.42b) 
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while  the  partial  derivatives  for  platform  b  are  the  negative  of  the  corresponding 
partial  derivatives  for  platform  a; 


dz 

dxb 

dz 

dyb 

dz 

dzb 


xb  -  xa 
z 

Vb  ~  Va 

z 

Zb  -  Zg 
z 


(3.42c) 

(3.42d) 

(3.42e) 


The  final  variable  needed  for  a  range  measurement  is  the  measurement  covari¬ 
ance.  As  the  state  estimates  the  position  error  of  platform  a,  the  error  of  the  mea¬ 
surement  device  is  redefined  as: 


V*  =  HabbPb  +  V 


(3.43) 


and  the  error  measurement  becomes: 


S  z  =  Haa5pa  +  v* 


(3.44) 


The  error  measurement  covariance  is  computed  as  the  expected  value  of  the 
measurement  device  error: 


R* 


E 


v  v 


E  [Hab6pbpl  H[6  +  Habp6u  +  v1  pbftTab  +  vvT] 
H abE  [pbpb]  +  0  +  0  +  E[vvt] 
HafoRPi>Pi)Hafe  +  R 


(3.45) 

(3.46) 

(3.47) 


The  next  section  develops  the  secondary  local  filter  algorithm  using  the  matched 
landmark  and  possible  range  measurements. 
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3.7  Secondary  Local  Filter 

The  secondary  local  filter  provides  a  platform  with  the  ability  to  incorporate 
information  about  landmarks  viewed  by  remote  platforms  and  the  relative  positioning 
of  platforms  cooperating  in  the  navigation  system.  The  filter  calculates  the  error  state 
to  measurement  state  transformation  matrix,  measurement  covariance,  and  residuals 
then  performs  a  measurement  update. 

The  filter  is  simply  the  measurement  update  step  of  an  EKF  and  operates  as 
described  in  Section  2.5  with  one  addition.  Whenever  a  new  landmark  match  is  added 
to  the  measurement  state  of  the  filter,  the  process  covariance  must  be  reset  to  an  initial 
condition.  This  procedure  is  performed  by  multiplying  the  process  covariance  matrix 
by  the  mask  matrix,  provided  by  the  primary  filter,  and  then  adding  the  initialization 
matrix: 

P  =  P„M  +  L  (3.48) 

where  P*  is  the  covariance  matrix  after  landmark  initialization,  PQ  is  the  covariance 
matrix  before  landmark  and  initialization,  M  is  the  masked  matrix,  and  L  is  the 
initialization  matrix.  The  secondary  local  filter  does  not  perform  the  propagation  step 
of  an  EKF  as  the  propagation  is  accomplished  by  the  primary  filter  of  the  platform 
providing  the  measurement.  Performing  a  second  propagation  will  increase  the  error 
in  the  state  estimate. 

When  the  filter  completes  a  measurement  update  the  results  are  sent  to  the 
master  filter  for  fusion  with  the  primary  filter  results. 

3.8  Master  Filter 

The  master  filter  is  designed  to  fuse  the  results  of  the  primary  and  secondary 
filters.  The  implementation  of  the  master  filter  is  dependent  on  the  method  of  imple¬ 
mentation  of  the  federated  filter,  as  discussed  in  Section  3.1,  and  the  federated  filter 
reset  mode.  The  filter  scales  the  process  covariances,  as  required  by  the  implemen- 
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tation  method;  fuses  the  error  states  and  process  covariances;  and  resets  all  platform 
filters,  as  required  by  the  reset  mode. 

3.8.1  Covariance  Scaling.  Scaling  of  the  process  covariances  is  performed 
only  when  the  post-scaling  method  is  implemented.  The  scaling  is  accomplished  by 
multiplying  the  process  covariances  of  the  primary  and  secondary  filters  by  the  scaling 
factor  (7): 


P 


S 

V 


7  pP 


P 


s 

s 


7  ps 


(3.49) 

(3.50) 


where  the  superscript  s  indicates  the  scaled  covariance,  subscript  p  indicates  the 
primary  filter,  and  subscript  s  indicates  the  secondary  filter. 

3.8.2  Information  Fusion.  Information  fusion  is  performed  using  the  infor¬ 
mation  form  of  federated  filter  as  this  form  is  the  simplest  to  implement  and  does  not 
slow  down  the  processing  of  the  filter.  Applying  the  additive  information  algorithm 
(2.85),  presented  in  Section  2.7.1,  provides: 

p-1  =  p-1  +  py  +  py  (3.5i) 

P~'x  =  p-'x,,,  +  p-'iip  +  Pj'xp  (3.52) 

when  the  filter  is  operated  in  fusion-reset  mode,  and: 

p_1  =  p;1  +  p;1  (3.53) 

P-U  -  P pXp  +  P«  1  x,  (3.54) 

in  no-reset  mode.  The  subscripts  m,  p,  and  s  represent  the  master,  primary,  and 
secondary  filters  respectively. 
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The  covariance  (P)  is  calculated  simply  by  inverting  the  information  matrix 
(P_1).  However,  the  error  state  vector  (x)  is  found  by  pre-multiplying  the  results  by 
the  covariance; 

x  =  P  (P-1x)  (3.55) 

3.8.3  Filter  Reset.  The  results  of  the  information  fusion  are  scaled  and 
applied  to  the  filters  differently  depending  on  the  reset  mode  of  operation. 

For  the  no-reset  mode  the  scaling  parameter  7  =  1  and  the  master  filter  is  the 
only  filter  reset.  In  this  mode  the  master  filter  only  fuses  the  information  provided 
by  the  primary  and  secondary  filters  and  does  not  retain  any  information  beyond  the 
current  time  step.  A  failure  or  lack  of  measurements  in  the  secondary  filter  causes 
the  overall  results  to  be  identical  to  the  original  non-cooperative  system. 

The  fusion-reset  mode  uses  a  scaling  parameter  7  =  3  and  applies  the  results 
to  all  three  filters.  As  a  result  each  filter  retains  a  portion  of  the  information  and  the 
primary  filter  gains  information  improving  the  overall  results  if  the  secondary  filter 
fails  or  does  not  receive  measurements. 

In  the  next  chapter  the  multiple  platform,  cooperative  navigation  system  devel¬ 
oped  here  is  validated  using  Monte  Carlo  simulation. 
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IV.  Simulation  Results  and  Observations 


The  cooperative,  vision-aided  navigation  system,  developed  in  Chapter  III,  is  eval¬ 
uated  using  Mat  lab®  simulation  and  an  incremental  verification  philosophy  is 
applied  to  demonstrate  the  accuracy  of  the  system.  This  chapter  begins  with  a  de¬ 
scription  of  the  filter  setup,  which  is  common  to  both  implementation  methods.  The 
remainder  of  the  chapter  is  sectioned  into  separate  analyses  of  the  post-scaling  and 
pre-scaling  methods  presented  in  Section  3.1.  Additionally,  each  section  is  divided 
into  separate  discussions  of  the  simulation  scenarios  applied  to  the  individual  imple¬ 
mentation  methods. 

All  of  the  results  presented  in  this  chapter  are  for  the  first  platform  in  the  coop¬ 
erative  system.  The  results  for  additional  platforms  are  presented  in  Appendices  A-E. 

4-1  Filter  Setup 

In  this  section  the  filter  implementation  common  to  all  simulations  is  described. 

4-1.1  Primary  Filter.  The  primary  local  filter  is  implemented  as  an  extended 
Kalman  filter  and  estimates  15  platform  states  and  36  landmark  states.  The  15 
platform  states  are  composed  of  three  states  each  representing  the  position,  velocity, 
attitude,  accelerometer  biases,  and  a  gyroscope  biases.  The  36  landmark  states  are 
composed  of  three  position  states  for  each  of  12  landmarks  tracked  and  estimated  by 
the  system. 

Measurements  are  provided  to  the  filter  from  a  binocular  vision  system  and  are 
comprised  of  landmark  positions  within  an  image.  The  addition  of  new  landmarks  is 
limited  to  six  landmarks  per  image.  When  the  vision  system  is  no  longer  able  to  find 
a  tracked  landmark,  the  corresponding  states  are  propagated  for  one  second  before  a 
new  landmark  is  selected  and  tracked. 

4-1.2  Secondary  Filter.  The  secondary  local  filter  is  implemented  as  the 
measurement  update  step  of  a  Kalman  filter  and  estimates  the  same  51  platform  and 
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landmark  states  as  the  primary  local  filter.  The  federated  filter  allows  the  secondary 
filter  to  estimate  additional  landmark  states,  corresponding  to  extra  landmark  mea¬ 
surements  from  a  second  platform.  However,  this  ability  is  not  implemented  as  it 
requires  adaptive  scaling  of  the  local  filter  information  matrices  and  the  master  state 
vector  and  information  matrix. 

Landmark  measurements  are  provided  to  the  filter  from  other  platforms  and  are 
limited  to  the  12  landmarks  tracked  by  the  primary  filter. 

Range  measurements,  between  platforms,  are  generated  as  the  actual  distance 
between  the  platforms  with  an  added  random  Gaussian  error  with: 

fj,  =  0  m  (4.1) 

cr  =  5  cm  (4.2) 

The  generated  range  measurements  are  not  representative  of  any  specific  ranging 
sensor,  and  are  adjusted  to  fit  the  scale  used  in  the  simulation  scenarios. 

4-1.3  Master  Filter.  The  master  filter  estimates  all  of  the  states  estimated 
by  either  of  the  local  filters.  As  the  secondary  local  filter  is  estimating  the  same  states 
as  the  primary,  the  master  filter  state  vector  is  composed  of  the  same  51  states. 

The  local  filter  covariances  are  inverted  to  information  matrices  prior  to  fu¬ 
sion.  This  allows  for  simpler  implementation  with  the  greatly  reduced  possibility  of 
implementation  or  coding  errors. 

4-2  Method  1:  Post- Scaling 

The  post-scaling  method,  developed  in  Section  3.1,  is  simulated  as  a  simple  case 
comprised  of  two  stationary  platforms.  Both  platforms  are  placed  along  the  east- 
west  axis,  1  m  west  and  1  m  east  of  the  n-frame  origin  respectively.  Thirty  features, 
normally  distributed  in  each  direction,  are  centered  about  the  coordinate  [10,0,0]T 
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with  a  standard  deviation  of  3  m.  Figure  4.1  depicts  the  simulation  scenario.  The  50 
run  Monte  Carlo  simulation  is  performed  over  200  s  at  a  2  Hz  sampling  rate. 

The  position,  velocity  and  attitude  errors  relating  to  the  n- frame  north,  east, 
and  down  directions  are  shown  in  Figures  4.2-4.10.  As  expected,  the  vision-aiding  sys¬ 
tem  constrains  the  drift  of  the  INS.  However,  comparison  of  the  cooperative  systems 
to  the  non-cooperative  system  is  difficult,  using  separate  plots  of  the  errors.  Therefore, 
the  root-mean-squared  (RMS)  position,  velocity  and  attitude  errors  are  computed  to 
provide  one-dimensional  metrics  for  the  comparison  of  the  three  systems.  The  RMS 
errors  are  shown  in  Figures  4.11-4.21. 

Using  the  post-scaling  method  to  implement  the  cooperative  systems  displays  no 
improvement  of  the  navigation  solution  over  the  non-cooperative  system.  This  simu¬ 
lation  computes  the  solution  of  the  primary  filter  at  every  time  step  prior  to  applying 
the  results  to  the  secondary  and  master  filters.  Therefore,  there  is  no  mechanism 
for  applying  the  information,  provided  by  additional  platforms,  to  the  primary  filter. 
Without  the  addition  of  external  information  the  system  reverts  to  a  non-cooperative 
state.  This  results  is  caused  by  either  the  predetermination  of  the  primary  result  at 
every  time  step  or  subtle  coding  errors  in  the  simulation. 

The  pre-scaling  simulation  overcomes  the  limitations  of  the  post-scaling  simula¬ 
tion  by  computing  a  total  solution  at  each  time  step  prior  to  continuing  to  the  next. 
The  results  of  this  method  are  presented  in  the  next  section. 
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Figure  4.1:  Two  platform  stationary  simulation  scenario  using  the  post-scaling 
method.  The  platforms  are  each  positioned  1  m  from  the  origin  (red  lines)  along 
the  east-west  axis.  Thirty  random  features  (blue  asterisk)  are  positioned  around  the 
coordinate  [10,0,0]T. 
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(b)  Cooperative  Simulation  without  Ranging 


Time  (s) 

(c)  Cooperative  Simulation  with  Ranging 

Figure  4.2:  Simulated  50-run  Monte  Carlo  position  error  results  along  the  north  axis 
of  the  local  navigation  frame.  The  results  shown  are  for  the  first  of  two  stationary 
platforms  simulated  using  three  system  implementations  of  the  post-scaling  method: 
1)  non-cooperative  (top),  2)  cooperative  without  range  measurements  (middle),  and 
3)  cooperative  with  range  measurements  (bottom).  The  position  errors  of  each  run 
are  represented  by  the  blue  dotted  lines.  The  ensemble  mean  is  indicated  by  the  green 
line  while  the  ensemble  standard  deviation  is  indicated  by  the  red  lines. 
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Figure  4.3:  Simulated  50-run  Monte  Carlo  position  error  results  along  the  east  axis 
of  the  local  navigation  frame.  The  results  shown  are  for  the  first  of  two  stationary 
platforms  simulated  using  three  system  implementations  of  the  post-scaling  method: 
1)  non-cooperative  (top),  2)  cooperative  without  range  measurements  (middle),  and 
3)  cooperative  with  range  measurements  (bottom).  The  position  errors  of  each  run 
are  represented  by  the  blue  dotted  lines.  The  ensemble  mean  is  indicated  by  the  green 
line  while  the  ensemble  standard  deviation  is  indicated  by  the  red  lines. 
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Figure  4.4:  Simulated  50-run  Monte  Carlo  position  error  results  along  the  down  axis 
of  the  local  navigation  frame.  The  results  shown  are  for  the  first  of  two  stationary 
platforms  simulated  using  three  system  implementations  of  the  post-scaling  method: 
1)  non-cooperative  (top),  2)  cooperative  without  range  measurements  (middle),  and 
3)  cooperative  with  range  measurements  (bottom).  The  position  errors  of  each  run 
are  represented  by  the  blue  dotted  lines.  The  ensemble  mean  is  indicated  by  the  green 
line  while  the  ensemble  standard  deviation  is  indicated  by  the  red  lines. 
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Figure  4.5:  Simulated  50-run  Monte  Carlo  velocity  error  results  along  the  north  axis 
of  the  local  navigation  frame.  The  results  shown  are  for  the  first  of  two  stationary 
platforms  simulated  using  three  system  implementations  of  the  post-scaling  method: 
1)  non-cooperative  (top),  2)  cooperative  without  range  measurements  (middle),  and 
3)  cooperative  with  range  measurements  (bottom).  The  velocity  errors  of  each  run 
are  represented  by  the  blue  dotted  lines.  The  ensemble  mean  is  indicated  by  the  green 
line  while  the  ensemble  standard  deviation  is  indicated  by  the  red  lines. 
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Figure  4.6:  Simulated  50-run  Monte  Carlo  velocity  error  results  along  the  east  axis 
of  the  local  navigation  frame.  The  results  shown  are  for  the  first  of  two  stationary 
platforms  simulated  using  three  system  implementations  of  the  post-scaling  method: 
1)  non-cooperative  (top),  2)  cooperative  without  range  measurements  (middle),  and 
3)  cooperative  with  range  measurements  (bottom).  The  velocity  errors  of  each  run 
are  represented  by  the  blue  dotted  lines.  The  ensemble  mean  is  indicated  by  the  green 
line  while  the  ensemble  standard  deviation  is  indicated  by  the  red  lines. 
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Figure  4.7:  Simulated  50-run  Monte  Carlo  velocity  error  results  along  the  down  axis 
of  the  local  navigation  frame.  The  results  shown  are  for  the  first  of  two  stationary 
platforms  simulated  using  three  system  implementations  of  the  post-scaling  method: 
1)  non-cooperative  (top),  2)  cooperative  without  range  measurements  (middle),  and 
3)  cooperative  with  range  measurements  (bottom).  The  velocity  errors  of  each  run 
are  represented  by  the  blue  dotted  lines.  The  ensemble  mean  is  indicated  by  the  green 
line  while  the  ensemble  standard  deviation  is  indicated  by  the  red  lines. 
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(b)  Cooperative  Simulation  without  Ranging 
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Figure  4.8:  Simulated  50-run  Monte  Carlo  attitude  error  results  about  the  north  axis 
of  the  local  navigation  frame.  The  results  shown  are  for  the  first  of  two  stationary 
platforms  simulated  using  three  system  implementations  of  the  post-scaling  method: 
1)  non-cooperative  (top),  2)  cooperative  without  range  measurements  (middle),  and 
3)  cooperative  with  range  measurements  (bottom).  The  attitude  errors  of  each  run 
are  represented  by  the  blue  dotted  lines.  The  ensemble  mean  is  indicated  by  the  green 
line  while  the  ensemble  standard  deviation  is  indicated  by  the  red  lines. 
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Figure  4.9:  Simulated  50-run  Monte  Carlo  attitude  error  results  about  the  east  axis 
of  the  local  navigation  frame.  The  results  shown  are  for  the  first  of  two  stationary 
platforms  simulated  using  three  system  implementations  of  the  post-scaling  method: 
1)  non-cooperative  (top),  2)  cooperative  without  range  measurements  (middle),  and 
3)  cooperative  with  range  measurements  (bottom).  The  attitude  errors  of  each  run 
are  represented  by  the  blue  dotted  lines.  The  ensemble  mean  is  indicated  by  the  green 
line  while  the  ensemble  standard  deviation  is  indicated  by  the  red  lines. 
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Figure  4.10:  Simulated  50-run  Monte  Carlo  attitude  error  results  about  the  down  axis 
of  the  local  navigation  frame.  The  results  shown  are  for  the  first  of  two  stationary 
platforms  simulated  using  three  system  implementations  of  the  post-scaling  method: 
1)  non-cooperative  (top),  2)  cooperative  without  range  measurements  (middle),  and 
3)  cooperative  with  range  measurements  (bottom).  The  attitude  errors  of  each  run 
are  represented  by  the  blue  dotted  lines.  The  ensemble  mean  is  indicated  by  the  green 
line  while  the  ensemble  standard  deviation  is  indicated  by  the  red  lines. 
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Figure  4.11:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  position  error 
results  along  the  north  axis  of  the  local  navigation  frame.  The  results  shown  are  for 
the  first  of  two  stationary  platforms  simulated  using  three  system  implementations 
of  the  post-scaling  method:  1)  non-cooperative  (blue),  2)  cooperative  without  range 
measurements  (green),  and  3)  cooperative  with  range  measurements  (red).  Note:  the 
three  system  results  presented  here  are  identical  due  to  the  method  used  to  implement 
the  primary  filter. 
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Figure  4.12:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  position  error 
results  along  the  east  axis  of  the  local  navigation  frame.  The  results  shown  are  for 
the  first  of  two  stationary  platforms  simulated  using  three  system  implementations 
of  the  post-scaling  method:  1)  non-cooperative  (blue),  2)  cooperative  without  range 
measurements  (green),  and  3)  cooperative  with  range  measurements  (red).  Note:  the 
three  system  results  presented  here  are  identical  due  to  the  method  used  to  implement 
the  primary  filter. 
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Figure  4.13:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  position  error 
results  along  the  down  axis  of  the  local  navigation  frame.  The  results  shown  are  for 
the  first  of  two  stationary  platforms  simulated  using  three  system  implementations 
of  the  post-scaling  method:  1)  non-cooperative  (blue),  2)  cooperative  without  range 
measurements  (green),  and  3)  cooperative  with  range  measurements  (red).  Note:  the 
three  system  results  presented  here  are  identical  due  to  the  method  used  to  implement 
the  primary  filter. 
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Figure  4.14:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  absolute  posi¬ 
tion  error  results.  The  results  shown  are  for  the  first  of  two  stationary  platforms  sim¬ 
ulated  using  three  system  implementations:  1)  non-cooperative  (blue),  2)  cooperative 
without  range  measurements  (green),  and  3)  cooperative  with  range  measurements 
(red).  Note:  the  three  system  results  presented  here  are  identical  due  to  the  method 
used  to  implement  the  primary  filter. 
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Figure  4.15:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  velocity  error 
results  along  the  north  axis  of  the  local  navigation  frame.  The  results  shown  are  for 
the  first  of  two  stationary  platforms  simulated  using  three  system  implementations 
of  the  post-scaling  method:  1)  non-cooperative  (blue),  2)  cooperative  without  range 
measurements  (green),  and  3)  cooperative  with  range  measurements  (red).  Note:  the 
three  system  results  presented  here  are  identical  due  to  the  method  used  to  implement 
the  primary  filter. 
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Figure  4.16:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  velocity  error 
results  along  the  east  axis  of  the  local  navigation  frame.  The  results  shown  are  for 
the  first  of  two  stationary  platforms  simulated  using  three  system  implementations 
of  the  post-scaling  method:  1)  non-cooperative  (blue),  2)  cooperative  without  range 
measurements  (green),  and  3)  cooperative  with  range  measurements  (red).  Note:  the 
three  system  results  presented  here  are  identical  due  to  the  method  used  to  implement 
the  primary  filter. 
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Figure  4.17:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  velocity  error 
results  along  the  down  axis  of  the  local  navigation  frame.  The  results  shown  are  for 
the  first  of  two  stationary  platforms  simulated  using  three  system  implementations 
of  the  post-scaling  method:  1)  non-cooperative  (blue),  2)  cooperative  without  range 
measurements  (green),  and  3)  cooperative  with  range  measurements  (red).  Note:  the 
three  system  results  presented  here  are  identical  due  to  the  method  used  to  implement 
the  primary  filter. 
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Figure  4.18:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  absolute  ve¬ 
locity  error  results  .  The  results  shown  are  for  the  first  of  two  stationary  platforms 
simulated  using  three  system  implementations  of  the  post-scaling  method:  1)  non- 
cooperative  (blue),  2)  cooperative  without  range  measurements  (green),  and  3)  co¬ 
operative  with  range  measurements  (red).  Note:  the  three  system  results  presented 
here  are  identical  due  to  the  method  used  to  implement  the  primary  filter. 
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Figure  4.19:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  attitude  error 
results  about  the  north  axis  of  the  local  navigation  frame.  The  results  shown  are  for 
the  first  of  two  stationary  platforms  simulated  using  three  system  implementations 
of  the  post-scaling  method:  1)  non-cooperative  (blue),  2)  cooperative  without  range 
measurements  (green),  and  3)  cooperative  with  range  measurements  (red).  Note:  the 
three  system  results  presented  here  are  identical  due  to  the  method  used  to  implement 
the  primary  filter. 
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Figure  4.20:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  attitude  error 
results  about  the  east  axis  of  the  local  navigation  frame.  The  results  shown  are  for 
the  first  of  two  stationary  platforms  simulated  using  three  system  implementations 
of  the  post-scaling  method:  1)  non-cooperative  (blue),  2)  cooperative  without  range 
measurements  (green),  and  3)  cooperative  with  range  measurements  (red).  Note:  the 
three  system  results  presented  here  are  identical  due  to  the  method  used  to  implement 
the  primary  filter. 
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Figure  4.21:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  attitude  error 
results  about  the  down  axis  of  the  local  navigation  frame.  The  results  shown  are  for 
the  first  of  two  stationary  platforms  simulated  using  three  system  implementations 
of  the  post-scaling  method:  1)  non-cooperative  (blue),  2)  cooperative  without  range 
measurements  (green),  and  3)  cooperative  with  range  measurements  (red).  Note:  the 
three  system  results  presented  here  are  identical  due  to  the  method  used  to  implement 
the  primary  filter. 
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4-3  Method  2:  Pre- Scaling 


The  pre-scaling  algorithm,  developed  in  Section  3.1,  is  implemented  using  four 
simulation  scenarios:  1)  two  stationary  platforms,  2)  two  platforms  passing  through 
a,  25  m  long  hallway,  3)  two  platforms  orbiting  a  point  in  space,  and  4)  two  platforms 
performing  a  360°  yaw  in  a  10  m  radius  circular  room. 

4-3.1  Stationary  Platforms.  The  first  simulation  performed  with  the  pre¬ 
scaling  algorithm  repeats  the  simulation  performed  using  the  post-scaling  method. 
Figure  4.1  depicts  the  simulation  scenario.  The  50  run  Monte  Carlo  simulation  is 
performed  over  200  s  at  a  2  Hz  sampling  rate. 

The  RMS  errors  for  the  stationary  platform  simulation  are  shown  in  Figures  4.22- 
4.32.  As  expected,  the  cooperative  systems  reduce  the  error  of  the  navigation  solution 
position  and  velocity.  Additionally,  the  inclusion  of  ranging  information  reduces  the 
error  more  than  the  standard  cooperative  system.  Figure  4.25  provides  a  single  indica¬ 
tor  of  the  overall  position  performance  of  the  filters.  The  cooperative  systems  reduce 
the  absolute  position  error  by  approximately  0.5  cm  without  ranging  information  and 
1  cm  with  ranging. 

The  cooperative  systems  do  not  improve  the  attitude  error,  as  seen  in  Fig¬ 
ures  4.30-4.32,  and  this  additional  error  is  due  to  the  unchanging  pose  of  the  two 
platforms.  The  cooperative  systems  increase  the  attitude  errors  by  as  much  as  0.01 
radians,  or  6°,  in  any  one  direction.  The  reasons  for  the  added  attitude  error  and  a 
technique  to  correct  it  are  discussed  in  Section  4.3.5. 
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Figure  4.22:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  position  error 
results  along  the  north  axis  of  the  local  navigation  frame.  The  results  shown  are  for 
the  first  of  two  stationary  platforms  simulated  using  three  system  implementations 
of  the  pre-scaling  method:  1)  non-cooperative  (blue),  2)  cooperative  without  range 
measurements  (green),  and  3)  cooperative  with  range  measurements  (red). 
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Figure  4.23:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  position  error 
results  along  the  east  axis  of  the  local  navigation  frame.  The  results  shown  are  for 
the  first  of  two  stationary  platforms  simulated  using  three  system  implementations 
of  the  pre-scaling  method:  1)  non-cooperative  (blue),  2)  cooperative  without  range 
measurements  (green),  and  3)  cooperative  with  range  measurements  (red). 
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Figure  4.24:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  position  error 
results  along  the  down  axis  of  the  local  navigation  frame.  The  results  shown  are  for 
the  first  of  two  stationary  platforms  simulated  using  three  system  implementations 
of  the  pre-scaling  method:  1)  non-cooperative  (blue),  2)  cooperative  without  range 
measurements  (green),  and  3)  cooperative  with  range  measurements  (red). 
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Figure  4.25:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  absolute  po¬ 
sition  error  results.  The  results  shown  are  for  the  first  of  two  stationary  platforms 
simulated  using  three  system  implementations  of  the  pre-scaling  method:  1)  non- 
cooperative  (blue),  2)  cooperative  without  range  measurements  (green),  and  3)  coop¬ 
erative  with  range  measurements  (red). 
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Figure  4.26:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  velocity  error 
results  along  the  north  axis  of  the  local  navigation  frame.  The  results  shown  are  for 
the  first  of  two  stationary  platforms  simulated  using  three  system  implementations 
of  the  pre-scaling  method:  1)  non-cooperative  (blue),  2)  cooperative  without  range 
measurements  (green),  and  3)  cooperative  with  range  measurements  (red). 
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Figure  4.27:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  velocity  error 
results  along  the  east  axis  of  the  local  navigation  frame.  The  results  shown  are  for 
the  first  of  two  stationary  platforms  simulated  using  three  system  implementations 
of  the  pre-scaling  method:  1)  non-cooperative  (blue),  2)  cooperative  without  range 
measurements  (green),  and  3)  cooperative  with  range  measurements  (red). 
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Figure  4.28:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  velocity  error 
results  along  the  down  axis  of  the  local  navigation  frame.  The  results  shown  are  for 
the  first  of  two  stationary  platforms  simulated  using  three  system  implementations 
of  the  pre-scaling  method:  1)  non-cooperative  (blue),  2)  cooperative  without  range 
measurements  (green),  and  3)  cooperative  with  range  measurements  (red). 


Figure  4.29:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  absolute  ve¬ 
locity  error  results  .  The  results  shown  are  for  the  first  of  two  stationary  platforms 
simulated  using  three  system  implementations  of  the  pre-scaling  method:  1)  non- 
cooperative  (blue),  2)  cooperative  without  range  measurements  (green),  and  3)  coop¬ 
erative  with  range  measurements  (red). 
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Figure  4.30:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  attitude  error 
results  about  the  north  axis  of  the  local  navigation  frame.  The  results  shown  are  for 
the  first  of  two  stationary  platforms  simulated  using  three  system  implementations 
of  the  pre-scaling  method:  1)  non-cooperative  (blue),  2)  cooperative  without  range 
measurements  (green),  and  3)  cooperative  with  range  measurements  (red). 


Figure  4.31:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  attitude  error 
results  about  the  east  axis  of  the  local  navigation  frame.  The  results  shown  are  for 
the  first  of  two  stationary  platforms  simulated  using  three  system  implementations 
of  the  pre-scaling  method:  1)  non-cooperative  (blue),  2)  cooperative  without  range 
measurements  (green),  and  3)  cooperative  with  range  measurements  (red). 
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Figure  4.32:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  attitude  error 
results  about  the  down  axis  of  the  local  navigation  frame.  The  results  shown  are  for 
the  first  of  two  stationary  platforms  simulated  using  three  system  implementations 
of  the  pre-scaling  method:  1)  non-cooperative  (blue),  2)  cooperative  without  range 
measurements  (green),  and  3)  cooperative  with  range  measurements  (red). 
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4-3.2  Hallway.  The  second  simulation,  using  the  pre-scaling  algorithm,  is 
designed  to  simulate  a  real-world  environment  and  consists  of  two  platforms  traversing 
15  m  of  a  25  m  long  simulated  hallway.  The  hallway  is  2  m  wide  and  3  m  tall  beginning 
5  m  south  of  the  n- frame  origin  and  and  ending  20  m  north  of  the  origin.  Eighty 
features  are  randomly  placed  along  the  walls,  floor  and  ceiling  of  the  hallway. 

The  first  platform  starts  at  the  coordinate  [0,  —0.5,  0.5]r,  while  the  second  plat¬ 
form  starts  at  [0,  0.5,  — 0.5]T.  Both  platforms  hold  these  positions  for  80  seconds 
and  then  accelerate  to  the  north  0.025  m/s  for  15  seconds.  Then,  at  120  seconds 
the  platforms  accelerate  0.025  m/s  to  the  south  for  another  15  seconds  ending  at 
[15,  — 0.5,0. 5]t  and  [15,0.5,  — 0.5]T  respectively.  The  platforms  then  hold  these  posi¬ 
tions  for  the  remainder  of  the  200  second  simulation.  Figure  4.33  depicts  the  simula¬ 
tion  scenario. 

The  simulation  results  are  shown  in  Figures  4.34-4.44.  As  with  the  stationary 
simulation,  the  attitude  errors  are  increased  by  the  cooperative  systems.  However, 
due  to  the  motion  of  the  platforms  beginning  at  80  seconds,  errors  in  the  north  axis 
are  cross  coupled  into  the  east  and  down  axes.  This  results  in  error  baises,  shown  in 
Figures  4.35  and  4.36,  after  the  platforms  traverse  the  hallway. 
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Figure  4.33:  Two  platform  moving  platform  simulation  scenario  using  the  pre-scaling 
method.  The  platforms  each  travel  (black  lines)  15m  along  a  2m  wide,  3m  tall,  and 
25  m  long  hallway.  Eighty  random  features  (blue  asterisk)  are  positioned  randomly 
on  the  walls,  floor,  and  ceiling  of  the  hallway. 


92 


0.07 


Figure  4.34:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  position  error 
results  along  the  north  axis  of  the  local  navigation  frame.  The  results  shown  are  for 
the  first  of  two  platforms,  traveling  along  a  25  m  hallway,  simulated  using  three  system 
implementations  of  the  pre-scaling  method:  1)  non-cooperative  (blue),  2)  cooperative 
without  range  measurements  (green),  and  3)  cooperative  with  range  measurements 
(red). 
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Figure  4.35:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  position  error 
results  along  the  east  axis  of  the  local  navigation  frame.  The  results  shown  are  for  the 
first  of  two  platforms,  traveling  along  a  25  m  hallway,  simulated  using  three  system 
implementations  of  the  pre-scaling  method:  1)  non-cooperative  (blue),  2)  cooperative 
without  range  measurements  (green),  and  3)  cooperative  with  range  measurements 
(red). 
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Figure  4.36:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  position  error 
results  along  the  down  axis  of  the  local  navigation  frame.  The  results  shown  are  for 
the  first  of  two  platforms,  traveling  along  a  25  m  hallway,  simulated  using  three  system 
implementations  of  the  pre-scaling  method:  1)  non-cooperative  (blue),  2)  cooperative 
without  range  measurements  (green),  and  3)  cooperative  with  range  measurements 
(red). 
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Figure  4.37:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  absolute  po¬ 
sition  error  results.  The  results  shown  are  for  the  first  of  two  platforms,  traveling 
along  a  25  m  hallway,  simulated  using  three  system  implementations  of  the  pre¬ 
scaling  method:  1)  non-cooperative  (blue),  2)  cooperative  without  range  measure¬ 
ments  (green),  and  3)  cooperative  with  range  measurements  (red). 
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Figure  4.38:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  velocity  error 
results  along  the  north  axis  of  the  local  navigation  frame.  The  results  shown  are  for 
the  first  of  two  platforms,  traveling  along  a  25  m  hallway,  simulated  using  three  system 
implementations  of  the  pre-scaling  method:  1)  non-cooperative  (blue),  2)  cooperative 
without  range  measurements  (green),  and  3)  cooperative  with  range  measurements 
(red). 
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Figure  4.39:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  velocity  error 
results  along  the  east  axis  of  the  local  navigation  frame.  The  results  shown  are  for  the 
first  of  two  platforms,  traveling  along  a  25  m  hallway,  simulated  using  three  system 
implementations  of  the  pre-scaling  method:  1)  non-cooperative  (blue),  2)  cooperative 
without  range  measurements  (green),  and  3)  cooperative  with  range  measurements 
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Figure  4.40:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  velocity  error 
results  along  the  down  axis  of  the  local  navigation  frame.  The  results  shown  are  for 
the  first  of  two  platforms,  traveling  along  a  25  m  hallway,  simulated  using  three  system 
implementations  of  the  pre-scaling  method:  1)  non-cooperative  (blue),  2)  cooperative 
without  range  measurements  (green),  and  3)  cooperative  with  range  measurements 
(red). 
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Figure  4.41:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  absolute  ve¬ 
locity  error  results  .  The  results  shown  are  for  the  first  of  two  platforms,  travel¬ 
ing  along  a  25  m  hallway,  simulated  using  three  system  implementations  of  the  pre¬ 
scaling  method:  1)  non-cooperative  (blue),  2)  cooperative  without  range  measure¬ 
ments  (green),  and  3)  cooperative  with  range  measurements  (red). 
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Figure  4.42:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  attitude  error 
results  about  the  north  axis  of  the  local  navigation  frame.  The  results  shown  are  for 
the  first  of  two  platforms,  traveling  along  a  25  m  hallway,  simulated  using  three  system 
implementations  of  the  pre-scaling  method:  1)  non-cooperative  (blue),  2)  cooperative 
without  range  measurements  (green),  and  3)  cooperative  with  range  measurements 
(red). 
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Figure  4.43:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  attitude  error 
results  about  the  east  axis  of  the  local  navigation  frame.  The  results  shown  are  for  the 
first  of  two  platforms,  traveling  along  a  25  m  hallway,  simulated  using  three  system 
implementations  of  the  pre-scaling  method:  1)  non-cooperative  (blue),  2)  cooperative 
without  range  measurements  (green),  and  3)  cooperative  with  range  measurements 
(red). 


102 


0.03 


Figure  4.44:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  attitude  error 
results  about  the  down  axis  of  the  local  navigation  frame.  The  results  shown  are  for 
the  first  of  two  platforms,  traveling  along  a  25  m  hallway,  simulated  using  three  system 
implementations  of  the  pre-scaling  method:  1)  non-cooperative  (blue),  2)  cooperative 
without  range  measurements  (green),  and  3)  cooperative  with  range  measurements 
(red). 
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4-3.3  Moving  Platforms.  The  third  simulation,  using  the  pre-scaling  algo¬ 
rithm,  tests  the  cooperative  systems’  abilities  to  compute  solutions  of  moving  plat¬ 
forms.  The  two  platforms  perform  sideslip  orbits  of  the  reference  feature  cloud.  The 
sideslip  orbit  is  characterized  by  the  orientation  of  the  platform  as  it  circles  a  point 
in  space.  At  all  times  the  platform  is  oriented  so  the  x-axis  of  the  body  frame  of 
reference  is  pointed  at  the  center  point  of  the  orbit. 

The  first  platform  begins  and  ends  the  sideslip  orbit  1  m  above  the  n- frame 
origin  and  orbits  the  point  at  [10,  0,  —  1]T,  counterclockwise,  at  a  constant  altitude 
of  1  m  above  the  n-frame  origin.  The  second  platform  follows  an  identical  orbital 
path  lrn  below  the  n-frame  origin  and  starting  45°  clockwise  from  the  n-frame  origin. 
Each  platform  completes  one  full  orbit  of  the  feature  cloud  over  the  course  of  the 
simulation. 

The  simulation  utilizes  30  reference  features  distributed  in  the  same  manner  as 
the  stationary  simulations  performed  previously.  Figure  4.45  depicts  the  simulation 
scenario.  The  50  run  Monte  Carlo  simulation  is  performed  over  200  s  at  a  2  Hz 
sampling  rate. 

The  results  of  the  sideslip  simulation  are  shown  in  Figures  4.46-4.56.  As  ex¬ 
pected,  the  position  errors  are  reduced  by  the  cooperative  systems.  The  absolute 
position  is  improved  by  up  to  10  cm  depending  on  the  position  of  the  platforms  in  the 
orbit. 

This  simulation  demonstrates  how  changing  the  attitude  and  position,  of  the 
platforms,  eliminates  the  attitude  errors  displayed  by  the  stationary  and  hallway 
simulations.  The  reasoning  behind  this  result  is  discussed  in  Section  4.3.5. 
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Figure  4.45:  Two  platform  moving  platform  simulation  scenario  using  the  pre-scaling 
method.  The  platforms  each  perform  a  counterclockwise,  sideslip  orbit  (black  lines) 
of  the  central  point  of  the  feature  cloud  at  a  radius  of  10  m.  The  two  platforms  are 
separated  45°  around  the  orbit  and  1  m  above  and  below  the  local  navigation  frame 
x-y  axis  plane.  Thirty  random  features  (blue  asterisk)  are  positioned  around  the 
coordinate  [10,0,0]T. 
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Figure  4.46:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  position  error 
results  along  the  north  axis  of  the  local  navigation  frame.  The  results  shown  are  for 
the  first  of  two  platforms,  traveling  along  sideslip  orbits  around  an  object,  simulated 
using  three  system  implementations  of  the  pre-scaling  method:  1)  non-cooperative 
(blue),  2)  cooperative  without  range  measurements  (green),  and  3)  cooperative  with 
range  measurements  (red). 
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Figure  4.47:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  position  error 
results  along  the  east  axis  of  the  local  navigation  frame.  The  results  shown  are  for 
the  first  of  two  platforms  traveling  along  sideslip  orbits  around  an  object,  simulated 
using  three  system  implementations  of  the  pre-scaling  method:  1)  non-cooperative 
(blue),  2)  cooperative  without  range  measurements  (green),  and  3)  cooperative  with 
range  measurements  (red). 
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Figure  4.48:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  position  error 
results  along  the  down  axis  of  the  local  navigation  frame.  The  results  shown  are  for 
the  first  of  two  platforms,  traveling  along  sideslip  orbits  around  an  object,  simulated 
using  three  system  implementations  of  the  pre-scaling  method:  1)  non-cooperative 
(blue),  2)  cooperative  without  range  measurements  (green),  and  3)  cooperative  with 
range  measurements  (red). 
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Figure  4.49:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  absolute  po¬ 
sition  error  results.  The  results  shown  are  for  the  first  of  two  platforms,  traveling 
along  sideslip  orbits  around  an  object,  simulated  using  three  system  implementations 
of  the  pre-scaling  method:  1)  non-cooperative  (blue),  2)  cooperative  without  range 
measurements  (green),  and  3)  cooperative  with  range  measurements  (red). 
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Figure  4.50:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  velocity  error 
results  along  the  north  axis  of  the  local  navigation  frame.  The  results  shown  are  for 
the  first  of  two  platforms,  traveling  along  sideslip  orbits  around  an  object,  simulated 
using  three  system  implementations  of  the  pre-scaling  method:  1)  non-cooperative 
(blue),  2)  cooperative  without  range  measurements  (green),  and  3)  cooperative  with 
range  measurements  (red). 
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Figure  4.51:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  velocity  error 
results  along  the  east  axis  of  the  local  navigation  frame.  The  results  shown  are  for 
the  first  of  two  platforms,  traveling  along  sideslip  orbits  around  an  object,  simulated 
using  three  system  implementations  of  the  pre-scaling  method:  1)  non-cooperative 
(blue),  2)  cooperative  without  range  measurements  (green),  and  3)  cooperative  with 
range  measurements  (red). 
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Figure  4.52:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  velocity  error 
results  along  the  down  axis  of  the  local  navigation  frame.  The  results  shown  are  for 
the  first  of  two  platforms,  traveling  along  sideslip  orbits  around  an  object,  simulated 
using  three  system  implementations  of  the  pre-scaling  method:  1)  non-cooperative 
(blue),  2)  cooperative  without  range  measurements  (green),  and  3)  cooperative  with 
range  measurements  (red). 
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Figure  4.53:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  absolute  ve¬ 
locity  error  results  .  The  results  shown  are  for  the  first  of  two  platforms,  traveling 
along  sideslip  orbits  around  an  object,  simulated  using  three  system  implementations 
of  the  pre-scaling  method:  1)  non-cooperative  (blue),  2)  cooperative  without  range 
measurements  (green),  and  3)  cooperative  with  range  measurements  (red). 
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Figure  4.54:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  attitude  error 
results  about  the  north  axis  of  the  local  navigation  frame.  The  results  shown  are  for 
the  first  of  two  platforms,  traveling  along  sideslip  orbits  around  an  object,  simulated 
using  three  system  implementations  of  the  pre-scaling  method:  1)  non-cooperative 
(blue),  2)  cooperative  without  range  measurements  (green),  and  3)  cooperative  with 
range  measurements  (red). 
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Figure  4.55:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  attitude  error 
results  about  the  east  axis  of  the  local  navigation  frame.  The  results  shown  are  for 
the  first  of  two  platforms,  traveling  along  sideslip  orbits  around  an  object,  simulated 
using  three  system  implementations  of  the  pre-scaling  method:  1)  non-cooperative 
(blue),  2)  cooperative  without  range  measurements  (green),  and  3)  cooperative  with 
range  measurements  (red). 


115 


0.035 


Figure  4.56:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  attitude  error 
results  about  the  down  axis  of  the  local  navigation  frame.  The  results  shown  are  for 
the  first  of  two  platforms,  traveling  along  sideslip  orbits  around  an  object,  simulated 
using  three  system  implementations  of  the  pre-scaling  method:  1)  non-cooperative 
(blue),  2)  cooperative  without  range  measurements  (green),  and  3)  cooperative  with 
range  measurements  (red). 
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4-3-4  Yaw.  The  final  pre-scaling  simulation  is  designed  to  test  the  effect  of 
an  attitude  maneuver,  without  any  position  change,  to  provide  observability  of  the 
attitude  error  states. 

The  simulation  is  composed  of  two  platforms  positioned  1  m  above  and  below 
the  n-frame  origin  respectively.  The  platforms  begin  the  simulation  pointed  north  and 
rotate  around  the  down  axis  counterclockwise,  completing  one  complete  rotation  over 
the  200  s  simulation.  One  hundred  fifty  reference  features  are  uniformly  distributed 
around  a  cylinder,  centered  on  the  n-frame  origin,  with  a  height  and  radius  of  10  m. 
Figure  4.57  depicts  the  simulation  scenario. 

Figures  4.58-4.68  show  the  results  of  this  simulation.  These  results  are  used, 
with  the  prior  simulation  results,  in  the  next  section  to  explain  the  attitude  errors  in 
the  stationary  and  hallway  simulations. 
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Figure  4.57:  Two  platform  rotation  simulation  scenario  using  the  pre-scaling  method. 
The  platforms  each  perform  a  counterclockwise  360°  yaw  motion  over  the  course  of 
the  200  s  simulation.  The  two  platforms  begin  and  end  the  simulation  heading  north, 
positioned  1  m  above  and  below  the  local  navigation  frame  origin.  One  hundred  fifty 
reference  features  are  uniformly  distributed  around  a  cylinder,  centered  on  the  origin, 
with  a  height  and  radius  of  10  m. 
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Figure  4.58:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  position  er¬ 
ror  results  along  the  north  axis  of  the  local  navigation  frame.  The  results  shown 
are  for  the  first  of  two  platforms,  performing  a  360°  counterclockwise  yaw  withing 
a  10  m  radius  circular  room,  simulated  using  three  system  implementations  of  the 
pre-scaling  method:  1)  non-cooperative  (blue),  2)  cooperative  without  range  mea¬ 
surements  (green),  and  3)  cooperative  with  range  measurements  (red). 
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Figure  4.59:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  position  er¬ 
ror  results  along  the  east  axis  of  the  local  navigation  frame.  The  results  shown 
are  for  the  first  of  two  platforms,  performing  a  360°  counterclockwise  yaw  withing 
a  10  m  radius  circular  room,  simulated  using  three  system  implementations  of  the 
pre-scaling  method:  1)  non-cooperative  (blue),  2)  cooperative  without  range  mea¬ 
surements  (green),  and  3)  cooperative  with  range  measurements  (red). 
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Figure  4.60:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  position  er¬ 
ror  results  along  the  down  axis  of  the  local  navigation  frame.  The  results  shown 
are  for  the  first  of  two  platforms,  performing  a  360°  counterclockwise  yaw  withing 
a  10  m  radius  circular  room,  simulated  using  three  system  implementations  of  the 
pre-scaling  method:  1)  non-cooperative  (blue),  2)  cooperative  without  range  mea¬ 
surements  (green),  and  3)  cooperative  with  range  measurements  (red). 
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Figure  4.61:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  absolute  po¬ 
sition  error  results.  The  results  shown  are  for  the  first  of  two  platforms,  performing 
a  360°  counterclockwise  yaw  withing  a  10  m  radius  circular  room,  simulated  using 
three  system  implementations  of  the  pre-scaling  method:  1)  non-cooperative  (blue), 
2)  cooperative  without  range  measurements  (green),  and  3)  cooperative  with  range 
measurements  (red). 
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Figure  4.62:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  velocity  er¬ 
ror  results  along  the  north  axis  of  the  local  navigation  frame.  The  results  shown 
are  for  the  first  of  two  platforms,  performing  a  360°  counterclockwise  yaw  withing 
a  10  m  radius  circular  room,  simulated  using  three  system  implementations  of  the 
pre-scaling  method:  1)  non-cooperative  (blue),  2)  cooperative  without  range  mea¬ 
surements  (green),  and  3)  cooperative  with  range  measurements  (red). 
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Figure  4.63:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  velocity  er¬ 
ror  results  along  the  east  axis  of  the  local  navigation  frame.  The  results  shown 
are  for  the  first  of  two  platforms,  performing  a  360°  counterclockwise  yaw  withing 
a  10  m  radius  circular  room,  simulated  using  three  system  implementations  of  the 
pre-scaling  method:  1)  non-cooperative  (blue),  2)  cooperative  without  range  mea¬ 
surements  (green),  and  3)  cooperative  with  range  measurements  (red). 
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Figure  4.64:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  velocity  er¬ 
ror  results  along  the  down  axis  of  the  local  navigation  frame.  The  results  shown 
are  for  the  first  of  two  platforms,  performing  a  360°  counterclockwise  yaw  withing 
a  10  m  radius  circular  room,  simulated  using  three  system  implementations  of  the 
pre-scaling  method:  1)  non-cooperative  (blue),  2)  cooperative  without  range  mea¬ 
surements  (green),  and  3)  cooperative  with  range  measurements  (red). 


I  I  I  I  I  I  I  I  r 


■  Non-Cooperative 

■  Cooperative 
1  Range 


125 


0.35 


Figure  4.65:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  absolute  ve¬ 
locity  error  results  .  The  results  shown  are  for  the  first  of  two  platforms,  performing 
a  360°  counterclockwise  yaw  withing  a  10  m  radius  circular  room,  simulated  using 
three  system  implementations  of  the  pre-scaling  method:  1)  non-cooperative  (blue), 
2)  cooperative  without  range  measurements  (green),  and  3)  cooperative  with  range 
measurements  (red). 
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Figure  4.66:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  attitude  er¬ 
ror  results  about  the  north  axis  of  the  local  navigation  frame.  The  results  shown 
are  for  the  first  of  two  platforms,  performing  a  360°  counterclockwise  yaw  withing 
a  10  m  radius  circular  room,  simulated  using  three  system  implementations  of  the 
pre-scaling  method:  1)  non-cooperative  (blue),  2)  cooperative  without  range  mea¬ 
surements  (green),  and  3)  cooperative  with  range  measurements  (red). 
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Figure  4.67:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  attitude  er¬ 
ror  results  about  the  east  axis  of  the  local  navigation  frame.  The  results  shown 
are  for  the  first  of  two  platforms,  performing  a  360°  counterclockwise  yaw  withing 
a  10  m  radius  circular  room,  simulated  using  three  system  implementations  of  the 
pre-scaling  method:  1)  non-cooperative  (blue),  2)  cooperative  without  range  mea¬ 
surements  (green),  and  3)  cooperative  with  range  measurements  (red). 
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Figure  4.68:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  attitude  er¬ 
ror  results  about  the  down  axis  of  the  local  navigation  frame.  The  results  shown 
are  for  the  first  of  two  platforms,  performing  a  360°  counterclockwise  yaw  withing 
a  10  m  radius  circular  room,  simulated  using  three  system  implementations  of  the 
pre-scaling  method:  1)  non-cooperative  (blue),  2)  cooperative  without  range  mea¬ 
surements  (green),  and  3)  cooperative  with  range  measurements  (red). 
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4-3.5  Attitude  Errors.  The  results  of  the  four  simulations  show  two  factors 
combining  to  cause  the  increased  attitude  errors  seen  in  Figures  4.30-4.32  and  4.42- 
4.44.  These  two  factors  are  decreased  observability  of  the  platform’s  attitude  in  the 
landmark  positions,  and  the  scaling  of  local  filters  in  federated  filter. 

Figures  4.69-4.71  show  the  non-cooperative  system  results  from  the  four  simu¬ 
lations  performed  to  verify  the  operation  of  the  cooperative  systems.  These  results 
show  a  yaw  maneuver  redicing  the  pitch  and  roll  attitude  errors  of  the  non-cooperative 
system.  They  demonstrate  a  rotational  maneuver  improving  the  attitude  error  in  the 
two  opposite  directions. 

These  results  show  attitude  maneuvers  providing  observability  of  the  attitude 
in  the  landmark  information.  Therefore,  in  the  stationary  and  hallway  simulations, 
where  no  attitude  maneuvers  are  performed,  the  shared  landmark  information  con¬ 
tains  very  little  attitude  information. 

The  scaling  of  the  local  filter  information  in  a  federated  filter  provides  the  second 
factor  which  increases  the  attitude  errors.  Combining  the  reduced  attitude  observ¬ 
ability  with  the  local  filter  scaling  mechanism  creates  the  increased  attitude  errors  in 
the  stationary  and  hallway  simulations.  In  both  simulations  the  information  from  the 
primary  filter  is  halved  and  is  not  compensated  by  the  secondary  filter  as  the  land¬ 
mark  information  from  the  second  platform  contains  little  or  no  observable  attitude 
information. 

Theses  results  show  how  the  execution  of  a  yaw  maneuver,  in  the  sideslip  and 
yaw  simulations,  provides  increased  attitude  information  in  the  shared  landmark  in¬ 
formation  and  compensates  for  the  scaling  of  the  primary  filter  information.  However, 
these  results  may  be  the  result  of  the  simulations  scenarios  and  require  further  inves¬ 
tigation  to  determine  the  root  cause  for  the  attitude  errors. 

In  the  next  chapter,  conclusions  are  drawn  regarding  the  performance  of  the 
cooperative  systems  and  recommendations  are  presented  for  future  implementation 
of  this  research. 
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Figure  4.69:  Non-cooperative  system  root-mean-squared  (RMS)  attitude  error  results 
about  the  north  axis  of  the  local  navigation  frame.  The  results  shown  are  for  the 
stationary  (blue),  hallway  (green),  sideslip  (red),  and  yaw  (cyan)  simulations. 
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Figure  4.70:  Non-cooperative  system  root-mean-squared  (RMS)  attitude  error  results 
about  the  east  axis  of  the  local  navigation  frame.  The  results  shown  are  for  the 
stationary  (blue),  hallway  (green),  sideslip  (red),  and  yaw  (cyan)  simulations. 
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Figure  4.71:  Non-cooperative  system  root-mean-squared  (RMS)  attitude  error  results 
about  the  down  axis  of  the  local  navigation  frame.  The  results  shown  are  for  the 
stationary  (blue),  hallway  (green),  sideslip  (red),  and  yaw  (cyan)  simulations. 
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V.  Conclusions  and  Recommendations 


This  thesis  details  a  research  effort  focused  on  expanding  previous  research  into 
the  fusion  of  optical  and  inertial  sensors  for  robust,  autonomous  navigation  to 
multiple  platforms.  In  this  chapter,  conclusions  regarding  the  research  are  presented 
and  discussed  along  with  recommendations  for  potential  future  research. 

5.1  Conclusions 

The  most  significant  conclusion  for  this  research  is  the  demonstration  of  the 
viability  of  cooperative,  vision-aided  navigation  using  federated  filtering  techniques. 
The  goal  of  this  research  is  the  development  of  a  method  for  sharing  landmark  infor¬ 
mation,  acquired  through  vision  systems,  between  platforms  improving  the  navigation 
solutions.  The  concept  is  developed  into  two  possible  cooperative  navigation  systems 
and  tested  using  Matlab®  simulation. 

Both  navigation  systems  utilize  a  federated  filter  to  incorporate  landmark  in¬ 
formation,  and  possibly  platform  ranging  information,  from  secondary  platforms  into 
a  fused  total  solution.  The  first  system  is  designed  to  directly  leverage  the  algorithms 
for  a  single  platform  vision-aided  navigation  system  without  modifications.  This  sys¬ 
tem  requires  the  implementation  of  a  new  federated  filter  scaling  system  which  allows 
for  the  scaling  of  the  local  filters  after  the  propagation  and  measurement  update  steps 
are  performed.  The  second  system  is  designed  to  use  the  single  platform  algorithms 
after  modification  to  allow  for  a  direct  implementation  of  a  federated  filter. 

Simulation  of  the  first  navigation  system  shows  no  change  in  the  accuracy  of 
the  cooperative  system  compared  to  the  single  platform  or  non-cooperative  system. 
The  cooperative  system  is  unable  to  improve  on  the  non-cooperative  results  due  to  the 
method  by  which  the  primary  filter  is  implemented.  The  primary  filter  implements  the 
single  platform  algorithms  without  modification.  Therefore,  the  primary  filter  solution 
is  computed  at  every  time  step  before  the  solution  is  applied  to  the  master  filter.  This 
prohibits  the  inclusion  of  external  data  into  the  solution  to  improve  navigation  state 
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accuracy.  Further  investigation  is  required  to  determine  if  the  post-scaling  method 
developed  in  Chapter  III  is  valid. 

As  expected,  simulation  of  the  second  cooperative  navigation  system  demon¬ 
strates  an  ability  to  reduce  the  position  and  velocity  errors  over  a  non-cooperative 
vision-aided  system.  However,  when  no  rotational  motion  is  present  in  the  over¬ 
all  motion  of  the  platforms,  the  cooperative  systems  increase  the  attitude  errors  as 
demonstrated  in  Figures  4.30-4.32.  This  increase  of  the  attitude  errors  is  due  to  the 
scaling  of  the  local  filters  and  reduced  observability  of  the  attitude  information  in  the 
absence  of  rotational  motion.  Additionally,  when  platform  motion  is  limited  to  linear 
position  changes  the  attitude  errors  couple  the  position  errors  in  the  direction  of  mo¬ 
tion  to  the  off  axis  directions.  This  phenomenon  is  demonstrated  in  Figures  4.34-4.40. 

There  exist  two  methods  to  reduce  or  overcome  the  attitude  errors  caused  by 
a  lack  of  rotational  motion:  a)  adaptive  scaling  of  the  local  filters,  and  b)  inducing 
rotational  motion. 

Adaptive  scaling  of  the  local  filters  require  using  separate  scaling  parameters  for 
the  platform  and  landmark  portions  of  the  covariance  matrix.  The  scaling  parameter 
for  the  platform  portion  is  set  to  a  constant  while  the  scaling  parameter  for  the 
landmark  portion  is  adaptively  varied  dependent  on  the  amount  of  rotational  motion 
in  the  secondary  platform.  The  adaptive  scaling  method  will  reduce  the  amount  of 
information  provided  by  the  secondary  platform  and  increase  position  errors  while 
reducing  the  attitude  errors.  Additionally,  this  method  is  difficult  to  implement. 

The  introduction  of  rotational  motion  to  the  cooperating  platforms  is  a  much 
easier  solution  to  implement.  In  fact,  many  real  world  systems  may  naturally  include 
enough  rotational  motion  to  counteract  the  errors  shown  in  the  performed  simulations. 
However,  these  errors  should  not  be  overlooked  when  designing  control  systems  which 
are  provided  navigation  solutions  using  this  cooperative  navigation  system. 
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While  the  approach  developed  in  this  research  demonstrates  improved  perfor¬ 
mance,  over  non-cooperative  navigation  systems,  there  are  areas  which  merit  addi¬ 
tional  research.  These  areas  are  addressed  in  the  next  section. 

5.2  Recommendations 

This  section  recommends  and  briefly  discusses  three  areas  of  possible  interest 
for  future  research. 

The  first  area  of  future  research  is  to  investigate  the  effect  of  communication 
protocols  and  networking  techniques  upon  the  cooperative  navigation  system.  The 
research  presented  in  this  thesis  assumes  a  common  communication  protocol  and  does 
not  implement  any  form  of  networking.  Additionally,  the  simulations  performed  in 
Chapter  IV  implements  the  communication  between  platforms  instantaneously.  Any 
efforts  to  implement  the  cooperative  navigation  system  concept  on  real-world  systems 
must  account  for  the  latency  in  communications  and  network  topology. 

The  next  area  of  future  research  is  the  further  development  and  implementation 
of  the  concepts  developed  in  this  research  to  real-world  systems.  This  research  would 
first  entail  performing  new  simulations  using  data  collected  from  real-world  systems 
to  validate  the  cooperative  navigation  system  concepts  with  data  from  real  sensors. 
Next,  the  algorithms  developed  for  this  research  require  updates  to  allow  for  real-time 
implementation.  Finally,  implementation  of  the  real-time  algorithms  onto  unmanned 
vehicles  will  allow  for  the  experimental  validation  of  this  research. 

The  final  area  of  future  research  is  too  further  investigate  the  observability  of 
attitude  information  in  the  landmark  data.  As  discussed  in  the  previous  section,  the 
observability  of  attitude  information  in  the  landmarks  is  severely  reduced  when  the 
platforms  do  not  perform  rotational  maneuvers.  This  poses  a  problem  in  the  cooper¬ 
ative  navigation  system  when  linear  position  errors  are  increased  due  to  the  coupling 
of  errors  between  axes.  As  demonstrated  in  Section  4.3.2  increased  attitude  errors 
increased  the  coupling  of  position  errors  between  axes  and  reduce  the  effectiveness  of 
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the  cooperative  navigation  system.  This  research  would  entail  investigating  the  at¬ 
titude  observability  and  determining  the  optimal  means  for  mitigating  the  increased 
attitude  errors. 

5. 3  Summary 

This  thesis  presented  the  problem  of  expanding  past  research  into  the  coupling  of 
vision  and  inertial  navigation  systems  to  multiple  cooperating  platforms.  A  federated 
filter  was  designed  to  provide  a  solution  and  using  Matlab®  simulations  the  validity 
of  the  filter  was  explored. 

The  federated  filter  designed  in  this  thesis  is  a  first  step  in  the  development  of 
a  cooperative  navigation  system  utilizing  fused  vision  and  inertial  sensor  data.  The 
development  of  such  a  system  has  only  just  begun  and,  as  discussed  in  the  previous 
section,  there  is  abundant  work  required  to  realize  the  full  potential  of  this  technology. 
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Appendix  A.  Second  Platform  Simulation  Results:  Post-scaling 
Method  -  Stationary  Simulation 


Figure  A.l:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  position  error 
results  along  the  north  axis  of  the  local  navigation  frame.  The  results  shown  are  for 
the  first  of  two  stationary  platforms  simulated  using  three  system  implementations 
of  the  post-scaling  method:  1)  non-cooperative  (blue),  2)  cooperative  without  range 
measurements  (green),  and  3)  cooperative  with  range  measurements  (red).  Note:  the 
three  system  results  presented  here  are  identical  due  to  the  method  used  to  implement 
the  primary  filter. 
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Figure  A. 2:  Simulated  50-run  Monte  Carlo  root- mean-squared  (RMS)  position  error 
results  along  the  east  axis  of  the  local  navigation  frame.  The  results  shown  are  for 
the  first  of  two  stationary  platforms  simulated  using  three  system  implementations 
of  the  post-scaling  method:  1)  non-cooperative  (blue),  2)  cooperative  without  range 
measurements  (green),  and  3)  cooperative  with  range  measurements  (red).  Note:  the 
three  system  results  presented  here  are  identical  due  to  the  method  used  to  implement 
the  primary  filter. 
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Figure  A. 3:  Simulated  50-run  Monte  Carlo  root- mean-squared  (RMS)  position  error 
results  along  the  down  axis  of  the  local  navigation  frame.  The  results  shown  are  for 
the  first  of  two  stationary  platforms  simulated  using  three  system  implementations 
of  the  post-scaling  method:  1)  non-cooperative  (blue),  2)  cooperative  without  range 
measurements  (green),  and  3)  cooperative  with  range  measurements  (red).  Note:  the 
three  system  results  presented  here  are  identical  due  to  the  method  used  to  implement 
the  primary  filter. 
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Figure  A. 4:  Simulated  50-run  Monte  Carlo  root- mean-squared  (RMS)  absolute  posi¬ 
tion  error  results.  The  results  shown  are  for  the  first  of  two  stationary  platforms  sim¬ 
ulated  using  three  system  implementations:  1)  non-cooperative  (blue),  2)  cooperative 
without  range  measurements  (green),  and  3)  cooperative  with  range  measurements 
(red).  Note:  the  three  system  results  presented  here  are  identical  due  to  the  method 
used  to  implement  the  primary  filter. 
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Figure  A. 5:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  velocity  error 
results  along  the  north  axis  of  the  local  navigation  frame.  The  results  shown  are  for 
the  first  of  two  stationary  platforms  simulated  using  three  system  implementations 
of  the  post-scaling  method:  1)  non-cooperative  (blue),  2)  cooperative  without  range 
measurements  (green),  and  3)  cooperative  with  range  measurements  (red).  Note:  the 
three  system  results  presented  here  are  identical  due  to  the  method  used  to  implement 
the  primary  filter. 
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Figure  A. 6:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  velocity  error 
results  along  the  east  axis  of  the  local  navigation  frame.  The  results  shown  are  for 
the  first  of  two  stationary  platforms  simulated  using  three  system  implementations 
of  the  post-scaling  method:  1)  non-cooperative  (blue),  2)  cooperative  without  range 
measurements  (green),  and  3)  cooperative  with  range  measurements  (red).  Note:  the 
three  system  results  presented  here  are  identical  due  to  the  method  used  to  implement 
the  primary  filter. 


■  Non-Cooperative 

■  Cooperative 
1  Range 


j _ i _ i _ i _ i _ i _ i _ i _ i_ 


143 


0.07 


0.06 


0.05 


2 

w  0.04 

in 

Z 

QC 

£ 

■m  0.03 
> 

C 

E 

o 

0.02 


0.01 


0 

0  20  40  60  80  100  120  140  160  180  200 

Time  (s) 

Figure  A. 7:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  velocity  error 
results  along  the  down  axis  of  the  local  navigation  frame.  The  results  shown  are  for 
the  first  of  two  stationary  platforms  simulated  using  three  system  implementations 
of  the  post-scaling  method:  1)  non-cooperative  (blue),  2)  cooperative  without  range 
measurements  (green),  and  3)  cooperative  with  range  measurements  (red).  Note:  the 
three  system  results  presented  here  are  identical  due  to  the  method  used  to  implement 
the  primary  filter. 
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Figure  A. 8:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  absolute  ve¬ 
locity  error  results  .  The  results  shown  are  for  the  first  of  two  stationary  platforms 
simulated  using  three  system  implementations  of  the  post-scaling  method:  1)  non- 
cooperative  (blue),  2)  cooperative  without  range  measurements  (green),  and  3)  co¬ 
operative  with  range  measurements  (red).  Note:  the  three  system  results  presented 
here  are  identical  due  to  the  method  used  to  implement  the  primary  filter. 
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Figure  A. 9:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  attitude  error 
results  about  the  north  axis  of  the  local  navigation  frame.  The  results  shown  are  for 
the  first  of  two  stationary  platforms  simulated  using  three  system  implementations 
of  the  post-scaling  method:  1)  non-cooperative  (blue),  2)  cooperative  without  range 
measurements  (green),  and  3)  cooperative  with  range  measurements  (red).  Note:  the 
three  system  results  presented  here  are  identical  due  to  the  method  used  to  implement 
the  primary  filter. 
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Figure  A.  10:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  attitude  error 
results  about  the  east  axis  of  the  local  navigation  frame.  The  results  shown  are  for 
the  first  of  two  stationary  platforms  simulated  using  three  system  implementations 
of  the  post-scaling  method:  1)  non-cooperative  (blue),  2)  cooperative  without  range 
measurements  (green),  and  3)  cooperative  with  range  measurements  (red).  Note:  the 
three  system  results  presented  here  are  identical  due  to  the  method  used  to  implement 
the  primary  filter. 
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Figure  A. 11:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  attitude  error 
results  about  the  down  axis  of  the  local  navigation  frame.  The  results  shown  are  for 
the  first  of  two  stationary  platforms  simulated  using  three  system  implementations 
of  the  post-scaling  method:  1)  non-cooperative  (blue),  2)  cooperative  without  range 
measurements  (green),  and  3)  cooperative  with  range  measurements  (red).  Note:  the 
three  system  results  presented  here  are  identical  due  to  the  method  used  to  implement 
the  primary  filter. 
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Appendix  B.  Second  Platform  Simulation  Results:  Pre-scaling  Method 

-  Stationary  Simulation 
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Figure  B.l:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  position  error 
results  along  the  north  axis  of  the  local  navigation  frame.  The  results  shown  are  for 
the  first  of  two  stationary  platforms  simulated  using  three  system  implementations 
of  the  pre-scaling  method:  1)  non-cooperative  (blue),  2)  cooperative  without  range 
measurements  (green),  and  3)  cooperative  with  range  measurements  (red). 
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Figure  B.2:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  position  error 
results  along  the  east  axis  of  the  local  navigation  frame.  The  results  shown  are  for 
the  first  of  two  stationary  platforms  simulated  using  three  system  implementations 
of  the  pre-scaling  method:  1)  non-cooperative  (blue),  2)  cooperative  without  range 
measurements  (green),  and  3)  cooperative  with  range  measurements  (red). 
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Figure  B.3:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  position  error 
results  along  the  down  axis  of  the  local  navigation  frame.  The  results  shown  are  for 
the  first  of  two  stationary  platforms  simulated  using  three  system  implementations 
of  the  pre-scaling  method:  1)  non-cooperative  (blue),  2)  cooperative  without  range 
measurements  (green),  and  3)  cooperative  with  range  measurements  (red). 
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Figure  B.4:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  absolute  po¬ 
sition  error  results.  The  results  shown  are  for  the  first  of  two  stationary  platforms 
simulated  using  three  system  implementations  of  the  pre-scaling  method:  1)  non- 
cooperative  (blue),  2)  cooperative  without  range  measurements  (green),  and  3)  coop¬ 
erative  with  range  measurements  (red). 
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Figure  B.5:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  velocity  error 
results  along  the  north  axis  of  the  local  navigation  frame.  The  results  shown  are  for 
the  first  of  two  stationary  platforms  simulated  using  three  system  implementations 
of  the  pre-scaling  method:  1)  non-cooperative  (blue),  2)  cooperative  without  range 
measurements  (green),  and  3)  cooperative  with  range  measurements  (red). 
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Figure  B.6:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  velocity  error 
results  along  the  east  axis  of  the  local  navigation  frame.  The  results  shown  are  for 
the  first  of  two  stationary  platforms  simulated  using  three  system  implementations 
of  the  pre-scaling  method:  1)  non-cooperative  (blue),  2)  cooperative  without  range 
measurements  (green),  and  3)  cooperative  with  range  measurements  (red). 
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Figure  B.7:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  velocity  error 
results  along  the  down  axis  of  the  local  navigation  frame.  The  results  shown  are  for 
the  first  of  two  stationary  platforms  simulated  using  three  system  implementations 
of  the  pre-scaling  method:  1)  non-cooperative  (blue),  2)  cooperative  without  range 
measurements  (green),  and  3)  cooperative  with  range  measurements  (red). 
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Figure  B.8:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  absolute  ve¬ 
locity  error  results  .  The  results  shown  are  for  the  first  of  two  stationary  platforms 
simulated  using  three  system  implementations  of  the  pre-scaling  method:  1)  non- 
cooperative  (blue),  2)  cooperative  without  range  measurements  (green),  and  3)  coop¬ 
erative  with  range  measurements  (red). 
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Figure  B.9:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  attitude  error 
results  about  the  north  axis  of  the  local  navigation  frame.  The  results  shown  are  for 
the  first  of  two  stationary  platforms  simulated  using  three  system  implementations 
of  the  pre-scaling  method:  1)  non-cooperative  (blue),  2)  cooperative  without  range 
measurements  (green),  and  3)  cooperative  with  range  measurements  (red). 
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Figure  B.10:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  attitude  error 
results  about  the  east  axis  of  the  local  navigation  frame.  The  results  shown  are  for 
the  first  of  two  stationary  platforms  simulated  using  three  system  implementations 
of  the  pre-scaling  method:  1)  non-cooperative  (blue),  2)  cooperative  without  range 
measurements  (green),  and  3)  cooperative  with  range  measurements  (red). 
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Figure  B.ll:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  attitude  error 
results  about  the  down  axis  of  the  local  navigation  frame.  The  results  shown  are  for 
the  first  of  two  stationary  platforms  simulated  using  three  system  implementations 
of  the  pre-scaling  method:  1)  non-cooperative  (blue),  2)  cooperative  without  range 
measurements  (green),  and  3)  cooperative  with  range  measurements  (red). 
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Appendix  C.  Second  Platform  Simulation  Results:  Pre-scaling  Method 

-  Hallway  Simulation 
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Figure  C.l:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  position  error 
results  along  the  north  axis  of  the  local  navigation  frame.  The  results  shown  are  for 
the  first  of  two  platforms,  traveling  along  a  25  m  hallway,  simulated  using  three  system 
implementations  of  the  pre-scaling  method:  1)  non-cooperative  (blue),  2)  cooperative 
without  range  measurements  (green),  and  3)  cooperative  with  range  measurements 
(red). 
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Figure  C.2:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  position  error 
results  along  the  east  axis  of  the  local  navigation  frame.  The  results  shown  are  for  the 
first  of  two  platforms,  traveling  along  a  25  m  hallway,  simulated  using  three  system 
implementations  of  the  pre-scaling  method:  1)  non-cooperative  (blue),  2)  cooperative 
without  range  measurements  (green),  and  3)  cooperative  with  range  measurements 
(red). 
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Figure  C.3:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  position  error 
results  along  the  down  axis  of  the  local  navigation  frame.  The  results  shown  are  for 
the  first  of  two  platforms,  traveling  along  a  25  m  hallway,  simulated  using  three  system 
implementations  of  the  pre-scaling  method:  1)  non-cooperative  (blue),  2)  cooperative 
without  range  measurements  (green),  and  3)  cooperative  with  range  measurements 
(red). 
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Figure  C.4:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  absolute  po¬ 
sition  error  results.  The  results  shown  are  for  the  first  of  two  platforms,  traveling 
along  a  25  m  hallway,  simulated  using  three  system  implementations  of  the  pre¬ 
scaling  method:  1)  non-cooperative  (blue),  2)  cooperative  without  range  measure¬ 
ments  (green),  and  3)  cooperative  with  range  measurements  (red). 
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Figure  C.5:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  velocity  error 
results  along  the  north  axis  of  the  local  navigation  frame.  The  results  shown  are  for 
the  first  of  two  platforms,  traveling  along  a  25  m  hallway,  simulated  using  three  system 
implementations  of  the  pre-scaling  method:  1)  non-cooperative  (blue),  2)  cooperative 
without  range  measurements  (green),  and  3)  cooperative  with  range  measurements 
(red). 
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Figure  C.6:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  velocity  error 
results  along  the  east  axis  of  the  local  navigation  frame.  The  results  shown  are  for  the 
first  of  two  platforms,  traveling  along  a  25  m  hallway,  simulated  using  three  system 
implementations  of  the  pre-scaling  method:  1)  non-cooperative  (blue),  2)  cooperative 
without  range  measurements  (green),  and  3)  cooperative  with  range  measurements 
(red). 
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Figure  C.7:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  velocity  error 
results  along  the  down  axis  of  the  local  navigation  frame.  The  results  shown  are  for 
the  first  of  two  platforms,  traveling  along  a  25  m  hallway,  simulated  using  three  system 
implementations  of  the  pre-scaling  method:  1)  non-cooperative  (blue),  2)  cooperative 
without  range  measurements  (green),  and  3)  cooperative  with  range  measurements 
(red). 
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Figure  C.8:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  absolute  ve¬ 
locity  error  results  .  The  results  shown  are  for  the  first  of  two  platforms,  trav¬ 
eling  along  a  25  m  hallway,  simulated  using  three  system  implementations  of  the 
pre-scaling  method:  1)  non-cooperative  (blue),  2)  cooperative  without  range  mea¬ 
surements  (green),  and  3)  cooperative  with  range  measurements  (red). 
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Figure  C.9:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  attitude  error 
results  about  the  north  axis  of  the  local  navigation  frame.  The  results  shown  are  for 
the  first  of  two  platforms,  traveling  along  a  25  m  hallway,  simulated  using  three  system 
implementations  of  the  pre-scaling  method:  1)  non-cooperative  (blue),  2)  cooperative 
without  range  measurements  (green),  and  3)  cooperative  with  range  measurements 
(red). 
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Figure  C.10:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  attitude  error 
results  about  the  east  axis  of  the  local  navigation  frame.  The  results  shown  are  for  the 
first  of  two  platforms,  traveling  along  a  25  m  hallway,  simulated  using  three  system 
implementations  of  the  pre-scaling  method:  1)  non-cooperative  (blue),  2)  cooperative 
without  range  measurements  (green),  and  3)  cooperative  with  range  measurements 
(red). 
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Figure  C.ll:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  attitude  error 
results  about  the  down  axis  of  the  local  navigation  frame.  The  results  shown  are  for 
the  first  of  two  platforms,  traveling  along  a  25  m  hallway,  simulated  using  three  system 
implementations  of  the  pre-scaling  method:  1)  non-cooperative  (blue),  2)  cooperative 
without  range  measurements  (green),  and  3)  cooperative  with  range  measurements 
(red). 
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Appendix  D.  Second  Platform  Simulation  Results:  Pre-scaling  Method 


-  Sideslip  Simulation 


Figure  D.l:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  position  error 
results  along  the  north  axis  of  the  local  navigation  frame.  The  results  shown  are  for 
the  first  of  two  platforms,  traveling  along  sideslip  orbits  around  an  object,  simulated 
using  three  system  implementations  of  the  pre-scaling  method:  1)  non-cooperative 
(blue),  2)  cooperative  without  range  measurements  (green),  and  3)  cooperative  with 
range  measurements  (red). 
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Figure  D.2:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  position  error 
results  along  the  east  axis  of  the  local  navigation  frame.  The  results  shown  are  for 
the  first  of  two  platforms  traveling  along  sideslip  orbits  around  an  object,  simulated 
using  three  system  implementations  of  the  pre-scaling  method:  1)  non-cooperative 
(blue),  2)  cooperative  without  range  measurements  (green),  and  3)  cooperative  with 
range  measurements  (red). 
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Figure  D.3:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  position  error 
results  along  the  down  axis  of  the  local  navigation  frame.  The  results  shown  are  for 
the  first  of  two  platforms,  traveling  along  sideslip  orbits  around  an  object,  simulated 
using  three  system  implementations  of  the  pre-scaling  method:  1)  non-cooperative 
(blue),  2)  cooperative  without  range  measurements  (green),  and  3)  cooperative  with 
range  measurements  (red). 
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Figure  D.4:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  absolute  po¬ 
sition  error  results.  The  results  shown  are  for  the  first  of  two  platforms,  traveling 
along  sideslip  orbits  around  an  object,  simulated  using  three  system  implementations 
of  the  pre-scaling  method:  1)  non-cooperative  (blue),  2)  cooperative  without  range 
measurements  (green),  and  3)  cooperative  with  range  measurements  (red). 
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Figure  D.5:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  velocity  error 
results  along  the  north  axis  of  the  local  navigation  frame.  The  results  shown  are  for 
the  first  of  two  platforms,  traveling  along  sideslip  orbits  around  an  object,  simulated 
using  three  system  implementations  of  the  pre-scaling  method:  1)  non-cooperative 
(blue),  2)  cooperative  without  range  measurements  (green),  and  3)  cooperative  with 
range  measurements  (red). 
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Figure  D.6:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  velocity  error 
results  along  the  east  axis  of  the  local  navigation  frame.  The  results  shown  are  for 
the  first  of  two  platforms,  traveling  along  sideslip  orbits  around  an  object,  simulated 
using  three  system  implementations  of  the  pre-scaling  method:  1)  non-cooperative 
(blue),  2)  cooperative  without  range  measurements  (green),  and  3)  cooperative  with 
range  measurements  (red). 
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Figure  D.7:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  velocity  error 
results  along  the  down  axis  of  the  local  navigation  frame.  The  results  shown  are  for 
the  first  of  two  platforms,  traveling  along  sideslip  orbits  around  an  object,  simulated 
using  three  system  implementations  of  the  pre-scaling  method:  1)  non-cooperative 
(blue),  2)  cooperative  without  range  measurements  (green),  and  3)  cooperative  with 
range  measurements  (red). 


■  Non-Cooperative 

■  Cooperative 
1  Range 


J _ i _ i _ i _ i _ i _ i _ i _ l_ 


177 


0.35 


Figure  D.8:  Simulated  50-run  Monte  Carlo  root- mean-squared  (RMS)  absolute  ve¬ 
locity  error  results  .  The  results  shown  are  for  the  first  of  two  platforms,  traveling 
along  sideslip  orbits  around  an  object,  simulated  using  three  system  implementations 
of  the  pre-scaling  method:  1)  non-cooperative  (blue),  2)  cooperative  without  range 
measurements  (green),  and  3)  cooperative  with  range  measurements  (red). 
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Figure  D.9:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  attitude  error 
results  about  the  north  axis  of  the  local  navigation  frame.  The  results  shown  are  for 
the  first  of  two  platforms,  traveling  along  sideslip  orbits  around  an  object,  simulated 
using  three  system  implementations  of  the  pre-scaling  method:  1)  non-cooperative 
(blue),  2)  cooperative  without  range  measurements  (green),  and  3)  cooperative  with 
range  measurements  (red). 
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Figure  D.10:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  attitude  error 
results  about  the  east  axis  of  the  local  navigation  frame.  The  results  shown  are  for 
the  first  of  two  platforms,  traveling  along  sideslip  orbits  around  an  object,  simulated 
using  three  system  implementations  of  the  pre-scaling  method:  1)  non-cooperative 
(blue),  2)  cooperative  without  range  measurements  (green),  and  3)  cooperative  with 
range  measurements  (red). 
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Figure  D.ll:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  attitude  error 
results  about  the  down  axis  of  the  local  navigation  frame.  The  results  shown  are  for 
the  first  of  two  platforms,  traveling  along  sideslip  orbits  around  an  object,  simulated 
using  three  system  implementations  of  the  pre-scaling  method:  1)  non-cooperative 
(blue),  2)  cooperative  without  range  measurements  (green),  and  3)  cooperative  with 
range  measurements  (red). 
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Appendix  E.  Second  Platform  Simulation  Results:  Pre-scaling  Method 

-  Yaw  Simulation 


Figure  E.l:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  position  er¬ 
ror  results  along  the  north  axis  of  the  local  navigation  frame.  The  results  shown 
are  for  the  first  of  two  platforms,  performing  a  360°  counterclockwise  yaw  withing 
a  10  m  radius  circular  room,  simulated  using  three  system  implementations  of  the 
pre-scaling  method:  1)  non-cooperative  (blue),  2)  cooperative  without  range  mea¬ 
surements  (green),  and  3)  cooperative  with  range  measurements  (red). 
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Figure  E.2:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  position  er¬ 
ror  results  along  the  east  axis  of  the  local  navigation  frame.  The  results  shown 
are  for  the  first  of  two  platforms,  performing  a  360°  counterclockwise  yaw  withing 
a  10  m  radius  circular  room,  simulated  using  three  system  implementations  of  the 
pre-scaling  method:  1)  non-cooperative  (blue),  2)  cooperative  without  range  mea¬ 
surements  (green),  and  3)  cooperative  with  range  measurements  (red). 


183 


0.14 


Figure  E.3:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  position  er¬ 
ror  results  along  the  down  axis  of  the  local  navigation  frame.  The  results  shown 
are  for  the  first  of  two  platforms,  performing  a  360°  counterclockwise  yaw  withing 
a  10  m  radius  circular  room,  simulated  using  three  system  implementations  of  the 
pre-scaling  method:  1)  non-cooperative  (blue),  2)  cooperative  without  range  mea¬ 
surements  (green),  and  3)  cooperative  with  range  measurements  (red). 
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Figure  E.4:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  absolute  posi¬ 
tion  error  results.  The  results  shown  are  for  the  first  of  two  platforms,  performing 
a  360°  counterclockwise  yaw  withing  a  10  m  radius  circular  room,  simulated  using 
three  system  implementations  of  the  pre-scaling  method:  1)  non-cooperative  (blue), 
2)  cooperative  without  range  measurements  (green),  and  3)  cooperative  with  range 
measurements  (red). 
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Figure  E.5:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  velocity  er¬ 
ror  results  along  the  north  axis  of  the  local  navigation  frame.  The  results  shown 
are  for  the  first  of  two  platforms,  performing  a  360°  counterclockwise  yaw  withing 
a  10  m  radius  circular  room,  simulated  using  three  system  implementations  of  the 
pre-scaling  method:  1)  non-cooperative  (blue),  2)  cooperative  without  range  mea¬ 
surements  (green),  and  3)  cooperative  with  range  measurements  (red). 
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Figure  E.6:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  velocity  er¬ 
ror  results  along  the  east  axis  of  the  local  navigation  frame.  The  results  shown 
are  for  the  first  of  two  platforms,  performing  a  360°  counterclockwise  yaw  withing 
a  10  m  radius  circular  room,  simulated  using  three  system  implementations  of  the 
pre-scaling  method:  1)  non-cooperative  (blue),  2)  cooperative  without  range  mea¬ 
surements  (green),  and  3)  cooperative  with  range  measurements  (red). 
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Figure  E.7:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  velocity  er¬ 
ror  results  along  the  down  axis  of  the  local  navigation  frame.  The  results  shown 
are  for  the  first  of  two  platforms,  performing  a  360°  counterclockwise  yaw  withing 
a  10  m  radius  circular  room,  simulated  using  three  system  implementations  of  the 
pre-scaling  method:  1)  non-cooperative  (blue),  2)  cooperative  without  range  mea¬ 
surements  (green),  and  3)  cooperative  with  range  measurements  (red). 


■  Non-Cooperative 

■  Cooperative 
1  Range 


J _ i _ i _ i _ i _ i _ i _ i _ l_ 


188 


0.35 


Figure  E.8:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  absolute  veloc¬ 
ity  error  results  .  The  results  shown  are  for  the  first  of  two  platforms,  performing 
a  360°  counterclockwise  yaw  withing  a  10  m  radius  circular  room,  simulated  using 
three  system  implementations  of  the  pre-scaling  method:  1)  non-cooperative  (blue), 
2)  cooperative  without  range  measurements  (green),  and  3)  cooperative  with  range 
measurements  (red). 
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Figure  E.9:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  attitude  er¬ 
ror  results  about  the  north  axis  of  the  local  navigation  frame.  The  results  shown 
are  for  the  first  of  two  platforms,  performing  a  360°  counterclockwise  yaw  withing 
a  10  m  radius  circular  room,  simulated  using  three  system  implementations  of  the 
pre-scaling  method:  1)  non-cooperative  (blue),  2)  cooperative  without  range  mea¬ 
surements  (green),  and  3)  cooperative  with  range  measurements  (red). 
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Figure  E.10:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  attitude  er¬ 
ror  results  about  the  east  axis  of  the  local  navigation  frame.  The  results  shown 
are  for  the  first  of  two  platforms,  performing  a  360°  counterclockwise  yaw  withing 
a  10  m  radius  circular  room,  simulated  using  three  system  implementations  of  the 
pre-scaling  method:  1)  non-cooperative  (blue),  2)  cooperative  without  range  mea¬ 
surements  (green),  and  3)  cooperative  with  range  measurements  (red). 
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Figure  E.  11:  Simulated  50-run  Monte  Carlo  root-mean-squared  (RMS)  attitude  er¬ 
ror  results  about  the  down  axis  of  the  local  navigation  frame.  The  results  shown 
are  for  the  first  of  two  platforms,  performing  a  360°  counterclockwise  yaw  withing 
a  10  m  radius  circular  room,  simulated  using  three  system  implementations  of  the 
pre-scaling  method:  1)  non-cooperative  (blue),  2)  cooperative  without  range  mea¬ 
surements  (green),  and  3)  cooperative  with  range  measurements  (red). 
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